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

libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt

@@ -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

@@ -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

@@ -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)
+        val currentGround = requirePointOnEarth(orbital, fixedClvOrientation)
-            ?: BLHPoint(id.b, id.l, id.h)
         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)
     }