From e6822b6cafd36c880e13a621850d0270f5a71432 Mon Sep 17 00:00:00 2001
From: emelianov <troits@list.ru>
Date: Tue, 9 Jun 2026 13:41:50 +0300
Subject: [PATCH] =?UTF-8?q?=D0=9F=D0=A3=D0=A3=D0=94=20=D1=81=20=D0=BF?=
 =?UTF-8?q?=D0=BE=D1=81=D1=82=D0=BE=D1=8F=D0=BD=D0=BD=D1=8B=D0=BC=20=D0=B0?=
 =?UTF-8?q?=D0=B7=D0=B8=D0=BC=D1=83=D1=82=D0=BE=D0=BC?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../nstart/pcp/angularmotion/AzimuthPUUD.kt   | 24 ++++++++++++-------
 .../AngularMotionCalculatorSmokeTest.kt       |  8 ++++++-
 2 files changed, 23 insertions(+), 9 deletions(-)

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 ae941c5..1ea697a 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
@@ -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,
diff --git a/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt b/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt
index 29955e8..616f29c 100644
--- a/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt
+++ b/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
                 )
             )
         }