From ee68da30f2321fa18791b73c60a77df3c2d387da Mon Sep 17 00:00:00 2001 From: emelianov Date: Mon, 15 Jun 2026 16:23:27 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9F=D1=80=D0=BE=D0=B2=D0=B5=D1=80=D0=BA?= =?UTF-8?q?=D0=B0=20=D0=BD=D0=B0=20=D0=BF=D0=B5=D1=80=D0=B5=D1=81=D0=B5?= =?UTF-8?q?=D1=87=D0=B5=D0=BD=D0=B8=D0=B5=20=D0=9B=D0=92=20=D0=97=D0=B5?= =?UTF-8?q?=D0=BC=D0=BB=D0=B8=20=D0=BF=D1=80=D0=B8=20=D1=80=D0=B0=D1=81?= =?UTF-8?q?=D1=87=D0=B5=D1=82=D0=B5=20=D0=9F=D0=A3=D0=A3=D0=94?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../angularmotion/AbstractPuudCalculator.kt | 33 ++++++++++++++++--- .../nstart/pcp/angularmotion/AzimuthPUUD.kt | 13 ++++---- .../pcp/angularmotion/ConstOrientPUUD.kt | 7 ++-- 3 files changed, 39 insertions(+), 14 deletions(-) diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt index 0d8561c..987ff10 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt +++ b/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) diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt index 5caa82a..8cc184a 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt +++ b/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 } diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt index 1623262..aabb97f 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt +++ b/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) - ?: 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) }