ivan-igorevich/dc-observatio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Проверка на пересечение ЛВ Земли при расчете ПУУД

Browse Source
This commit is contained in:
emelianov
2026-06-15 16:23:27 +03:00
parent f49709d544
commit ee68da30f2
3 changed files with 39 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt
+29 -4
View File
@@ -176,6 +176,31 @@ abstract class AbstractPuudCalculator(
return earthIntersection(point.r, direction)
}
/**
* Обязательная проверка геометрической реализуемости ориентации.
*
* Во всех режимах ПУУД центральная линия визирования направлена вдоль -OY ПСК.
* Если при рассчитанной ориентации этот луч не пересекает ОЗЭ, то точка
* визирования, W/D, СДИ и контур съемки физически не определены. В таком
* случае расчет должен завершаться ошибкой, а не возвращать пустую точку
* визирования или нулевые скорости.
*/
protected fun requirePointOnEarth(point: OrbitalPoint, orientation: Orientation): BLHPoint =
pointOnEarth(point, orientation)
?: throw lineOfSightDoesNotIntersectEarth(point.t, orientation)
protected fun lineOfSightDoesNotIntersectEarth(
t: Double,
orientation: Orientation? = null,
): AngularMotionCalculationException {
val suffix = orientation?.let {
", tang=${it.tang}, kren=${it.kren}, risk=${it.risk}"
}.orEmpty()
return AngularMotionCalculationException(
"Линия визирования не пересекает Землю в момент t=$t$suffix",
)
}
protected fun earthIntersection(rotn: Vector3D, direction: Vector3D): BLHPoint? {
val d = direction.normSafe()
if ((rotn + d).module() > rotn.module()) return null
@@ -249,7 +274,7 @@ abstract class AbstractPuudCalculator(
val absToGsk = Matrix3D().also { it.makeOzMatrix(-astro.si2000(point.t)) }
val lineGsk = (absToGsk * orbitBookToAbs * orbitToOrbitBook * connectedToOrbit * lineInConnected).normSafe()
if (lineGsk.module() < EPS) return Vector3D()
if (lineGsk.module() < EPS) throw lineOfSightDoesNotIntersectEarth(point.t, orientation)
val earth = astro.earth
val kalpha = 1.0 - earth.alphaEllips * earth.ekvRadius / earth.ekvRadius
@@ -259,10 +284,10 @@ abstract class AbstractPuudCalculator(
val reducedLine2 = (reducedLine * reducedLine).coerceAtLeast(EPS)
val underRoot = reducedProjection * reducedProjection +
reducedLine2 * (earth.ekvRadius * earth.ekvRadius - reducedR * reducedR)
if (underRoot < 0.0 || !underRoot.isFinite()) return Vector3D()
if (underRoot < 0.0 || !underRoot.isFinite()) throw lineOfSightDoesNotIntersectEarth(point.t, orientation)
val range = -(reducedProjection + sqrt(underRoot)) / reducedLine2
if (!range.isFinite() || abs(range) < EPS) return Vector3D()
if (!range.isFinite() || range <= EPS) throw lineOfSightDoesNotIntersectEarth(point.t, orientation)
val slantGsk = lineGsk * range
val groundGsk = point.r + slantGsk
@@ -338,7 +363,7 @@ abstract class AbstractPuudCalculator(
val q = quaternionFor(orbital, orientation)
val omega = previous?.let { omegaFromTwoQuat(it.quaternion, q, t - it.t) } ?: Vector3D()
val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D()
val ground = pointOnEarth(orbital, orientation)
val ground = requirePointOnEarth(orbital, orientation)
val wd = wd(orbital, orientation, omega)
val edgeSdi = edgeSdiValues(orbital, orientation, omega, sickle)
val centerSdi = sdiForWd(wd, sickle)
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt
+7 -6
View File
@@ -85,7 +85,7 @@ open class AzimuthPUUD(
rAbs = ask.r,
reverse = sdiForTime < 0.0,
)
val di = slantRangeFromQuaternion(liv, ask.r)
val di = slantRangeFromQuaternion(liv, ask.r, t)
val omegaVisir = ownCornerSpeed(t, liv, di, ask.r, ask.v, sdiForTime, routeNormalGsk)
val orientation = orientationFromVisirQuaternion(orbital, liv)
val point = buildIntegratedPoint(t, orientation, omegaVisir, previous, sickle)
@@ -96,7 +96,7 @@ open class AzimuthPUUD(
liv = integrateQuaternionRK4(t, liv, step) { time, q ->
val p = pointAt(time)
val pAsk = astro.grinvToASK(p)
val pDi = slantRangeFromQuaternion(q, pAsk.r)
val pDi = slantRangeFromQuaternion(q, pAsk.r, time)
val pSdi = sdiAt(id.sdi, time - tn)
ownCornerSpeed(time, q, pDi, pAsk.r, pAsk.v, pSdi, routeNormalGsk)
}
@@ -149,7 +149,7 @@ open class AzimuthPUUD(
val orbital = pointAt(t)
val omega = visirToConnected(omegaVisir)
val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D()
val ground = pointOnEarth(orbital, orientation)
val ground = requirePointOnEarth(orbital, orientation)
val q = quaternionFor(orbital, orientation)
val wd = wd(orbital, orientation, omega)
val edgeSdi = edgeSdiValues(orbital, orientation, omega, sickle)
@@ -232,7 +232,7 @@ open class AzimuthPUUD(
val lineAbs = (liv * Vector3D(1.0, 0.0, 0.0)).normSafe()
if (lineAbs.module() < EPS) return liv
val di = slantRangeFromQuaternion(liv, rAbs)
val di = slantRangeFromQuaternion(liv, rAbs, time)
if (di.module() < EPS) return liv
val groundAbs = rAbs + di
@@ -265,14 +265,15 @@ open class AzimuthPUUD(
return (correction * liv).normalized()
}
protected fun slantRangeFromQuaternion(liv: Quaternion3D, rAbs: Vector3D): Vector3D {
protected fun slantRangeFromQuaternion(liv: Quaternion3D, rAbs: Vector3D, time: Double? = null): Vector3D {
val lineAbs = (liv * Vector3D(1.0, 0.0, 0.0)).normSafe()
val a = astro.earth.ekvRadius
val b = astro.earth.polarRadius
val aa = b * b * lineAbs.x * lineAbs.x + b * b * lineAbs.y * lineAbs.y + a * a * lineAbs.z * lineAbs.z
val bb = 2.0 * b * b * lineAbs.x * rAbs.x + 2.0 * b * b * lineAbs.y * rAbs.y + 2.0 * a * a * lineAbs.z * rAbs.z
val cc = b * b * rAbs.x * rAbs.x + b * b * rAbs.y * rAbs.y + a * a * rAbs.z * rAbs.z - a * a * b * b
val root = minPositiveRoot(aa, bb, cc) ?: return Vector3D()
val root = minPositiveRoot(aa, bb, cc)
?: throw lineOfSightDoesNotIntersectEarth(time ?: Double.NaN)
return lineAbs * root
}
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt
+3 -4
View File
@@ -134,7 +134,7 @@ class ConstOrientPUUD(
previous: AngularMotionPoint?,
): AngularMotionPoint {
val orbital = pointAt(t)
val ground = pointOnEarth(orbital, orientation)
val ground = requirePointOnEarth(orbital, orientation)
val q = quaternionFor(orbital, orientation)
val eps = previous?.let { if (kotlin.math.abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() }
?: Vector3D()
@@ -159,8 +159,7 @@ class ConstOrientPUUD(
fixedClvOrientation: Orientation,
): ConstSample {
val orbital = pointAt(t)
val currentGround = pointOnFixedClv(t, fixedClvOrientation)
?: BLHPoint(id.b, id.l, id.h)
val currentGround = requirePointOnEarth(orbital, fixedClvOrientation)
val routeDirectionGsk = fixedClvGroundDirection(t, step, fixedClvOrientation, currentGround)
val orientation = orientOnPoint(
t,
@@ -169,7 +168,7 @@ class ConstOrientPUUD(
currentGround.h,
routeDirectionGsk,
)
val ground = pointOnEarth(orbital, orientation) ?: currentGround
val ground = requirePointOnEarth(orbital, orientation)
val quaternion = quaternionFor(orbital, orientation)
return ConstSample(t, orbital, orientation, ground, quaternion)
}