ПУУД с постоянным азимутом

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

@@ -69,18 +69,22 @@ open class AzimuthPUUD(
 
         while (elapsed <= duration + EPS) {
             val orbital = pointAt(t)
+            val ask = astro.grinvToASK(orbital)
+            val di = slantRangeFromQuaternion(liv, ask.r)
+            val sdiForTime = sdiAt(id.sdi, elapsed)
+            val omegaVisir = ownCornerSpeed(t, liv, di, ask.r, ask.v, sdiForTime)
             val orientation = orientationFromVisirQuaternion(orbital, liv)
-            val point = buildIntegratedPoint(t, orientation, liv, previous, sickle)
+            val point = buildIntegratedPoint(t, orientation, omegaVisir, previous, sickle)
             result += point
             previous = point
 
             if (elapsed + step <= duration + EPS) {
                 liv = integrateQuaternionRK4(t, liv, step) { time, q ->
                     val p = pointAt(time)
-                    val ask = astro.grinvToASK(p)
-                    val di = slantRangeFromQuaternion(q, ask.r)
-                    val sdiForTime = sdiAt(id.sdi, time - tn)
-                    ownCornerSpeed(time, q, di, ask.r, ask.v, sdiForTime)
+                    val pAsk = astro.grinvToASK(p)
+                    val pDi = slantRangeFromQuaternion(q, pAsk.r)
+                    val pSdi = sdiAt(id.sdi, time - tn)
+                    ownCornerSpeed(time, q, pDi, pAsk.r, pAsk.v, pSdi)
                 }
             }
             elapsed += step
@@ -114,14 +118,15 @@ open class AzimuthPUUD(
     protected fun buildIntegratedPoint(
         t: Double,
         orientation: Orientation,
-        liv: Quaternion3D,
+        omegaVisir: Vector3D,
         previous: AngularMotionPoint?,
         sickle: Boolean,
     ): AngularMotionPoint {
         val orbital = pointAt(t)
-        val omega = previous?.let { omegaFromTwoQuat(it.quaternion, liv, t - it.t) } ?: Vector3D()
+        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 q = quaternionFor(orbital, orientation)
         val wd = wd(orbital, orientation, omega)
         return AngularMotionPoint(
             t = t,
@@ -130,12 +135,15 @@ open class AzimuthPUUD(
             groundPoint = ground,
             omega = omega,
             eps = eps,
-            quaternion = liv.normalized(),
+            quaternion = q,
             wd = wd,
             sdi = sdiForWd(wd, sickle),
         )
     }
 
+    private fun visirToConnected(omegaVisir: Vector3D): Vector3D =
+        conToVisirMatrix().transpose() * omegaVisir
+
     protected fun initialVisirQuaternion(
         rAbs: Vector3D,
         targetAbs: Vector3D,

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

@@ -200,13 +200,19 @@ class AngularMotionCalculatorSmokeTest {
             println(
                 String.format(
                     Locale.US,
-                    "%s %.3f %.3f %.3f , %s %s",
+                    "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f",
                     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
                 )
             )
         }