ПУУД

2026-06-04 13:22:48 +03:00
parent faedd21f43
commit b62a7cf8ef
3 changed files with 87 additions and 6 deletions
@@ -184,7 +184,9 @@ open class AzimuthPUUD(

    protected fun orientationFromVisirQuaternion(orbital: ballistics.types.OrbitalPoint, nlv: Int, liv: Quaternion3D): Orientation {
        val ask = astro.grinvToASK(orbital)
-        val absToOrbit = orbBookToOrbMatrix() * absToOrbBookMatrix(ask.r, ask.v)
+        val ob = orbBookToOrbMatrix()
+        val absOb = absToOrbBookMatrix(ask.r, ask.v)
+        val absToOrbit = ob * absOb
        val oepToConnected = rotationZ(-lineAngle(nlv)).transpose()
        val lConToVisir = Quaternion3D(0.0, sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0)
        val connectedToAbs = (liv * lConToVisir.inverse() * quaternionFromMatrix(oepToConnected).inverse()).matrix().transpose()
@@ -107,11 +107,21 @@ internal fun rotationY(angle: Double): Matrix3D = Matrix3D().also { it.makeOyMat
 * которая уже используется в ballistics.mpl.OrientOnPointCalculator.
 */
 internal fun absToOrbBookMatrix(rAbs: Vector3D, vAbs: Vector3D): Matrix3D {
-    val c = Matrix3D()
-    c.second = rAbs.normSafe()
-    c.third = (vAbs.rem(rAbs)).normSafe()
-    c.first = c.second.rem(c.third).normSafe()
-    return c
+//    val c = Matrix3D()
+//    c.second = rAbs.normSafe()
+//    c.third = (vAbs.rem(rAbs)).normSafe()
+//    c.first = c.second.rem(c.third).normSafe()
+    val c = rAbs.rem(vAbs)
+    val dummy = c.rem(rAbs)
+    val modC = c.module()
+    val modAbsRKA = rAbs.module()
+    val m = Matrix3D(
+        Vector3D( dummy.x/modC/modAbsRKA,c.x/modC, rAbs.x/modAbsRKA),
+        Vector3D( dummy.y/modC/modAbsRKA,c.y/modC, rAbs.y/modAbsRKA),
+        Vector3D( dummy.z/modC/modAbsRKA,c.z/modC, rAbs.z/modAbsRKA),
+
+    )
+    return m.transpose()
 }

 /** Матрица из OrbitalMotion::TLowOrbit::OrbBookToOrb. */
@@ -1,8 +1,16 @@
 package space.nstart.pcp.angularmotion

+import ballistics.Ballistics
 import ballistics.orbitalPoints.timeStepper.AbstractStepper
+import ballistics.types.BallisticsError
+import ballistics.types.EarthType
+import ballistics.types.InitialConditions
 import ballistics.types.OrbitalPoint
+import ballistics.utils.fromDateTime
 import ballistics.utils.math.Vector3D
+import ballistics.utils.toDateTime
+import java.time.LocalDateTime
+import kotlin.math.PI
 import kotlin.math.cos
 import kotlin.math.sin
 import kotlin.test.Test
@@ -71,4 +79,65 @@ class AngularMotionCalculatorSmokeTest {
            )
        }
    }
+
+
+
+    @Test
+    fun chek(){
+        val ballistics = Ballistics()
+        val t = LocalDateTime.of(2023, 4, 12, 17, 41, 18, 0)
+        val ic = InitialConditions(
+            OrbitalPoint(
+                fromDateTime(t),
+                1,
+                Vector3D(
+                    -3101926.630,
+                    6018678.8,
+                    0.0,
+                ),
+                Vector3D(
+                    1287.651,
+                    663.634,
+                    7612.951,
+                ),
+            ),
+            0.005,
+            160.0
+        )
+        val result = ballistics.calculateOrbPoints(
+            ic,
+            ic.point.t,
+            ic.point.t + 86400 * 10
+        )
+        assertEquals(result, BallisticsError.OK)
+        if (result != BallisticsError.OK)
+            return
+        val stepper = ballistics.getStepper()
+        assert(stepper != null)
+        if (stepper == null)
+            return
+        val calculator = AzimuthPUUD(stepper, EarthType.PZ90d02)
+        val id = SurveyId(
+            oep = listOf(false, false, false, true),
+            nlv = 5,
+            t = fromDateTime(LocalDateTime.of(2023, 4, 16, 3, 48, 22, 0)),
+            b = 49.25824 * PI / 180,
+            l = 153.65914 * PI / 180,
+            h = 0.0,
+            duration = 69.0,
+            sdi = listOf(30.0),
+            azimuth = -15.567 * PI / 180,
+            uprAngle = 0.0,
+            pointInCenter = false
+        )
+        val rc = calculator.calculate(id)
+
+        println(rc.mode)
+        println(rc.points.size)
+        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}")
+        }
+    }
 }