ПУУД с постоянной ориентацией

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

@@ -3,7 +3,9 @@ package space.nstart.pcp.angularmotion
 import ballistics.orbitalPoints.timeStepper.AbstractStepper
 import ballistics.types.BLHPoint
 import ballistics.types.EarthType
+import ballistics.types.OrbitalPoint
 import ballistics.types.Orientation
+import ballistics.utils.math.Quaternion3D
 import ballistics.utils.math.Vector3D
 
 /**
@@ -33,39 +35,143 @@ class ConstOrientPUUD(
     }
 
     private fun calculateConst(id: SurveyId, tn: Double, duration: Double): List<AngularMotionPoint> {
-        val result = mutableListOf<AngularMotionPoint>()
         val step = calculationStep()
+
+        // В режиме ConstOrient направление ЦЛВ в ОСК фиксируется на старте.
+        // Это задает постоянные крен и тангаж. Оставшаяся степень свободы —
+        // рысканье вокруг ЦЛВ. В отличие от геометрического варианта, где рысканье
+        // выбиралось по направлению трассы, здесь рысканье интегрируется из самого
+        // критерия Wz/D = 0. Такой подход совпадает по смыслу с AzimuthPUUD:
+        // сначала находится угловая скорость, при которой wd.z равен нулю, затем по
+        // этой скорости строится программа углового движения.
+        val fixedClvOrientation = orientOnPoint(tn, id.b, id.l, id.h)
+        val tang = fixedClvOrientation.tang
+        val kren = fixedClvOrientation.kren
+        var risk = buildConstSample(id, tn, step, fixedClvOrientation).orientation.risk
+
+        val result = mutableListOf<AngularMotionPoint>()
         var t = tn
         var elapsed = 0.0
         var previous: AngularMotionPoint? = null
 
-        // В режиме ConstOrient ЦЛВ не должна сопровождать одну и ту же земную точку.
+        while (elapsed <= duration + EPS) {
-        // На старте фиксируем направление ЦЛВ в ОСК, а далее на каждом шаге заново
+            val orientation = Orientation(tang, kren, normalizeAngle(risk))
-        // пересекаем эту фиксированную ЦЛВ с Землей. Важно, чтобы первая точка маршрута
+            val omega = omegaForWdzZero(t, orientation)
-        // тоже была получена через такое пересечение: иначе первый шаг сшивает заданную
+            val point = buildConstPoint(t, orientation, omega, previous)
-        // исходную ЦЛМ с расчетной трассой постоянной ЦЛВ и дает искусственный скачок рысканья.
-        val fixedClvOrientation = orientOnPoint(tn, id.b, id.l, id.h)
-
-        while (elapsed <= duration + 2.0 * step + EPS) {
-            val currentGround = pointOnFixedClv(t, fixedClvOrientation)
-                ?: BLHPoint(id.b, id.l, id.h)
-            val routeDirectionGsk = fixedClvGroundDirection(t, step, fixedClvOrientation, currentGround)
-            val orientation = orientOnPoint(
-                t,
-                currentGround.lat,
-                currentGround.long,
-                currentGround.h,
-                routeDirectionGsk,
-            )
-            val point = buildPoint(t, orientation, previous)
             result += point
             previous = point
 
+            if (elapsed + step <= duration + EPS) {
+                risk = integrateRiskRK4(t, risk, step, tang, kren)
+            }
             elapsed += step
             t += step
         }
 
-        return if (result.size > 2) result.dropLast(2) else result
+        return result
+    }
+
+    /**
+     * В AzimuthPUUD условие wd.z = 0 зашито непосредственно в формулы угловой
+     * скорости визирной системы. Для ConstOrient оставшаяся степень свободы только
+     * одна — производная рысканья. Поэтому в каждой точке решаем линейное уравнение
+     * по скорости рысканья:
+     *
+     *     wd.z(t, risk, riskDot) = 0.
+     *
+     * Функция wd линейна по omega, а omega линейна по riskDot, поэтому достаточно
+     * двух расчетов: при riskDot = 0 и при riskDot = 1 рад/с.
+     */
+    private fun riskRateForWdzZero(t: Double, orientation: Orientation): Double {
+        val point = pointAt(t)
+        val omega0 = omegaForRiskRate(t, orientation, 0.0)
+        val wz0 = wd(point, orientation, omega0).z
+        val omega1 = omegaForRiskRate(t, orientation, 1.0)
+        val wz1 = wd(point, orientation, omega1).z
+        val denom = wz1 - wz0
+        return if (!denom.isFinite() || kotlin.math.abs(denom) < WDZ_SOLVER_EPS) {
+            0.0
+        } else {
+            -wz0 / denom
+        }
+    }
+
+    private fun omegaForWdzZero(t: Double, orientation: Orientation): Vector3D =
+        omegaForRiskRate(t, orientation, riskRateForWdzZero(t, orientation))
+
+    /**
+     * Численно строит мгновенную угловую скорость ПСК при заданной скорости рысканья.
+     * Малый внутренний шаг нужен только для дифференцирования кватерниона; он не
+     * задает дискрет выдачи ПУУД.
+     */
+    private fun omegaForRiskRate(t: Double, orientation: Orientation, riskRate: Double): Vector3D {
+        val dt = OMEGA_DERIVATIVE_STEP
+        val q0 = quaternionFor(pointAt(t), orientation)
+        val nextOrientation = Orientation(
+            orientation.tang,
+            orientation.kren,
+            normalizeAngle(orientation.risk + riskRate * dt),
+        )
+        val q1 = quaternionFor(pointAt(t + dt), nextOrientation)
+        return omegaFromTwoQuat(q0, q1, dt)
+    }
+
+    private fun integrateRiskRK4(t: Double, risk: Double, step: Double, tang: Double, kren: Double): Double {
+        fun f(time: Double, value: Double): Double =
+            riskRateForWdzZero(time, Orientation(tang, kren, normalizeAngle(value)))
+
+        val k1 = f(t, risk)
+        val k2 = f(t + step / 2.0, risk + k1 * step / 2.0)
+        val k3 = f(t + step / 2.0, risk + k2 * step / 2.0)
+        val k4 = f(t + step, risk + k3 * step)
+        return normalizeAngle(risk + step * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0)
+    }
+
+    private fun buildConstPoint(
+        t: Double,
+        orientation: Orientation,
+        omega: Vector3D,
+        previous: AngularMotionPoint?,
+    ): AngularMotionPoint {
+        val orbital = pointAt(t)
+        val ground = pointOnEarth(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()
+        val wd = wd(orbital, orientation, omega)
+        return AngularMotionPoint(
+            t = t,
+            orbitalPoint = orbital,
+            orientation = Orientation(orientation.tang, orientation.kren, orientation.risk),
+            groundPoint = ground,
+            omega = omega,
+            eps = eps,
+            quaternion = q,
+            wd = wd,
+            sdi = sdiForWd(wd),
+        )
+    }
+
+    private fun buildConstSample(
+        id: SurveyId,
+        t: Double,
+        step: Double,
+        fixedClvOrientation: Orientation,
+    ): ConstSample {
+        val orbital = pointAt(t)
+        val currentGround = pointOnFixedClv(t, fixedClvOrientation)
+            ?: BLHPoint(id.b, id.l, id.h)
+        val routeDirectionGsk = fixedClvGroundDirection(t, step, fixedClvOrientation, currentGround)
+        val orientation = orientOnPoint(
+            t,
+            currentGround.lat,
+            currentGround.long,
+            currentGround.h,
+            routeDirectionGsk,
+        )
+        val ground = pointOnEarth(orbital, orientation) ?: currentGround
+        val quaternion = quaternionFor(orbital, orientation)
+        return ConstSample(t, orbital, orientation, ground, quaternion)
     }
 
     private fun pointOnFixedClv(t: Double, fixedClvOrientation: Orientation): BLHPoint? =
@@ -93,4 +199,17 @@ class ConstOrientPUUD(
         val second = astro.earth.blh2xyz(to.lat, to.long, to.h)
         return second - first
     }
+
+    private data class ConstSample(
+        val t: Double,
+        val orbitalPoint: OrbitalPoint,
+        val orientation: Orientation,
+        val groundPoint: BLHPoint?,
+        val quaternion: Quaternion3D,
+    )
+
+    private companion object {
+        const val WDZ_SOLVER_EPS = 1.0e-14
+        const val OMEGA_DERIVATIVE_STEP = 1.0e-2
+    }
 }

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

@@ -158,10 +158,35 @@ internal fun quaternionFromEuler(tang: Double, kren: Double, risk: Double): Quat
 internal fun omegaFromTwoQuat(q1: Quaternion3D, q2: Quaternion3D, step: Double): Vector3D {
     if (abs(step) < EPS) return Vector3D()
     val q1n = q1.normalized()
-    val dq = (q2.normalized() - q1n).scaled(1.0 / step)
+    val q2n = alignQuaternion(q1n, q2.normalized())
-    val l1 = Vector3D(q1n.q1, q1n.q2, q1n.q3)
+    val dq = (q2n - q1n).scaled(1.0 / step)
+    return omegaFromQuaternionDerivative(q1n, dq)
+}
+
+internal fun omegaFromThreeQuat(
+    qPrevious: Quaternion3D,
+    qCurrent: Quaternion3D,
+    qNext: Quaternion3D,
+    step: Double,
+): Vector3D {
+    if (abs(step) < EPS) return Vector3D()
+    val current = qCurrent.normalized()
+    val previous = alignQuaternion(current, qPrevious.normalized())
+    val next = alignQuaternion(current, qNext.normalized())
+    val dq = (next - previous).scaled(1.0 / (2.0 * step))
+    return omegaFromQuaternionDerivative(current, dq)
+}
+
+private fun alignQuaternion(reference: Quaternion3D, candidate: Quaternion3D): Quaternion3D {
+    val dot = reference.q0 * candidate.q0 + reference.q1 * candidate.q1 +
+        reference.q2 * candidate.q2 + reference.q3 * candidate.q3
+    return if (dot < 0.0) candidate.scaled(-1.0) else candidate
+}
+
+private fun omegaFromQuaternionDerivative(q: Quaternion3D, dq: Quaternion3D): Vector3D {
+    val l1 = Vector3D(q.q1, q.q2, q.q3)
     val l2 = Vector3D(dq.q1, dq.q2, dq.q3)
-    return (l1 * (-dq.q0) + l2 * q1n.q0 - (l1.rem(l2))) * 2.0
+    return (l1 * (-dq.q0) + l2 * q.q0 - (l1.rem(l2))) * 2.0
 }
 
 internal fun integrateQuaternionRK4(

libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt

@@ -138,7 +138,25 @@ class AngularMotionCalculatorSmokeTest {
         println(rc.startTime)
 
         for (v in rc.points) {
-            println("${toDateTime( v.t)} ${v.orientation.tang * 180 / PI} ${v.orientation.kren  * 180 / PI } ${v.orientation.risk  * 180 / PI}")
+            println(
+                String.format(
+                    Locale.US,
+                    "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f , %.3f",
+                    toDateTime(v.t).format(outputTimeFormatter),
+                    v.orientation.tang * 180 / PI,
+                    v.orientation.kren * 180 / PI,
+                    v.orientation.risk * 180 / PI,
+                    v.groundPoint?.let { String.format(Locale.US, "%.3f", it.lat * 180 / PI) },
+                    v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) },
+                    v.omega.x,
+                    v.omega.y,
+                    v.omega.z,
+                    v.wd.x,
+                    v.wd.y,
+                    v.wd.z,
+                    v.sdi
+                )
+            )
         }
     }
 
@@ -200,7 +218,7 @@ class AngularMotionCalculatorSmokeTest {
             println(
                 String.format(
                     Locale.US,
-                    "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f",
+                    "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f , %.3f",
                     toDateTime(v.t).format(outputTimeFormatter),
                     v.orientation.tang * 180 / PI,
                     v.orientation.kren * 180 / PI,
@@ -212,7 +230,8 @@ class AngularMotionCalculatorSmokeTest {
                     v.omega.z,
                     v.wd.x,
                     v.wd.y,
-                    v.wd.z
+                    v.wd.z,
+                    v.sdi
                 )
             )
         }