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

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

@@ -137,7 +137,7 @@ abstract class AbstractPuudCalculator(
         b: Double,
         l: Double,
         h: Double,
-        routeDirection: Vector3D? = null,
+        routeDirectionGsk: Vector3D? = null,
     ): Orientation {
         val ka = pointAt(t)
         val targetGsk = astro.earth.blh2xyz(b, l, h)
@@ -145,13 +145,40 @@ abstract class AbstractPuudCalculator(
         val compensated = rotationZ(-lineAngle(nlv)) * w
         val orient = orientationFromOrbitalVector(compensated)
 
-        if (routeDirection != null && routeDirection.module() > 1.0e-8) {
+        if (routeDirectionGsk != null && routeDirectionGsk.module() > 1.0e-8) {
-            val route = rotationZ(-lineAngle(nlv)) * routeDirection
+            orient.risk = riskFromGroundDirection(ka, nlv, orient, routeDirectionGsk)
-            orient.risk = normalizeAngle(atan2(route.x, route.z))
         }
         return orient
     }
 
+    /**
+     * Подбор рысканья по физическому смыслу условия Wz/D = 0.
+     *
+     * Крен и тангаж уже наводят ЦЛВ на расчетную точку. Оставшаяся степень свободы —
+     * поворот вокруг ЦЛВ. Переводим направление движения точки визирования с Земли в
+     * ПСК при нулевом рысканье и поворачиваем ПСК так, чтобы поперечная компонента по
+     * OZ стала равной нулю, то есть чтобы движение изображения шло вдоль OX.
+     */
+    private fun riskFromGroundDirection(
+        kaGsk: OrbitalPoint,
+        nlv: Int,
+        orientWithoutRisk: Orientation,
+        routeDirectionGsk: Vector3D,
+    ): Double {
+        val kaAbs = astro.grinvToASK(kaGsk)
+        val routeAbs = astro.grinvToASK(routeDirectionGsk, kaGsk.t)
+        val routeOrbit = orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v) * routeAbs
+        val routeConnected = orientationMatrix(orientWithoutRisk).transpose() * routeOrbit
+        val routeClv = rotationZ(-lineAngle(nlv)) * routeConnected
+        val wx = routeClv.x
+        val wz = routeClv.z
+        return if (sqrt(wx * wx + wz * wz) < 1.0e-10) {
+            0.0
+        } else {
+            normalizeAngle(atan2(-wz, wx))
+        }
+    }
+
     protected fun pointInOrbitalFrame(kaGsk: OrbitalPoint, pointGsk: Vector3D): Vector3D {
         val kaAbs = astro.grinvToASK(kaGsk)
         val pointAbs = astro.grinvToASK(pointGsk, kaGsk.t)
@@ -206,26 +233,59 @@ abstract class AbstractPuudCalculator(
 
     /**
      * Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для заданной линии визирования.
-     * Расчет оставлен самодостаточным и опирается на AstronomerJ2000/Vector3D/Matrix3D из ballistics-lib.
+     *
+     * W — относительная скорость движения по земной поверхности точки пересечения ЛВ с Землей.
+     * Это не скорость КА: из скорости спутника вычитается переносное вращение Земли, а затем
+     * добавляется радиальная составляющая изменения наклонной дальности, чтобы точка оставалась
+     * на поверхности эллипсоида. Такая схема соответствует формулам из OrbitalMotion::AbstractAISTPUUD::wd.
      */
     protected fun wd(point: OrbitalPoint, nlv: Int, orientation: Orientation, omegaConnected: Vector3D): Vector3D {
         val ask = astro.grinvToASK(point)
-        val line = lineOfSightVectorInConnected(lineAngle(nlv))
+        val lineConnected = lineOfSightVectorInConnected(lineAngle(nlv))
-        val bodyToOrbit = orientationMatrix(orientation)
+        val connectedToOrbit = orientationMatrix(orientation)
-        val lineOrbit = bodyToOrbit * line
+        val absToOrbBook = absToOrbBookMatrix(ask.r, ask.v)
-        val lineAbs = (absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose() * lineOrbit).normSafe()
+        val orbitBookToAbs = absToOrbBook.transpose()
-        val ground = earthIntersection(point.r, (Matrix3D().also { it.makeOzMatrix(-astro.si2000(point.t)) } * lineAbs).normSafe())
+        val orbitToOrbitBook = orbBookToOrbMatrix().transpose()
-            ?: return Vector3D()
+        val absToGsk = Matrix3D().also { it.makeOzMatrix(-astro.si2000(point.t)) }
-        val groundGsk = astro.earth.blh2xyz(ground)
+
-        val groundAbs = astro.grinvToASK(groundGsk, point.t)
+        val lineGsk = (absToGsk * orbitBookToAbs * orbitToOrbitBook * connectedToOrbit * lineConnected).normSafe()
-        val slantAbs = groundAbs - ask.r
+        if (lineGsk.module() < EPS) return Vector3D()
-        val range = slantAbs.module().coerceAtLeast(EPS)
+
-        val omegaEarth = Vector3D(0.0, 0.0, astro.earth.wEarth)
+        val earth = astro.earth
-        val relVelocityAbs = ask.v - omegaEarth.rem(ask.r)
+        val kalpha = 1.0 - earth.alphaEllips * earth.ekvRadius / earth.ekvRadius
-        val connectedToAbs = absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose() * orientationMatrix(orientation)
+        val reducedR = Vector3D(point.r.x, point.r.y, point.r.z / kalpha)
-        val relVelocityConnected = connectedToAbs.transpose() * relVelocityAbs
+        val reducedLine = Vector3D(lineGsk.x, lineGsk.y, lineGsk.z / kalpha)
-        val compensation = omegaConnected.rem(line) * range
+        val reducedProjection = reducedR * reducedLine
-        return (relVelocityConnected - compensation) / range
+        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()
+
+        val range = -(reducedProjection + sqrt(underRoot)) / reducedLine2
+        if (!range.isFinite() || abs(range) < EPS) return Vector3D()
+
+        val slantGsk = lineGsk * range
+        val groundGsk = point.r + slantGsk
+        val reducedGround = Vector3D(groundGsk.x, groundGsk.y, groundGsk.z / (kalpha * kalpha))
+
+        var omegaGsk = connectedToOrbit * omegaConnected
+        omegaGsk = orbitToOrbitBook * omegaGsk
+        omegaGsk = orbitBookToAbs * omegaGsk
+        omegaGsk = absToGsk * omegaGsk
+
+        val omegaEarth = Vector3D(0.0, 0.0, earth.wEarth)
+        val omegaRelativeToEarth = omegaGsk - omegaEarth
+        val relativeVelocity = point.v + omegaRelativeToEarth.rem(slantGsk)
+        val rangeRateCompensation = lineGsk * (-(reducedGround * relativeVelocity) / (reducedGround * lineGsk).coerceAwayFromZero())
+        var wGround = relativeVelocity + rangeRateCompensation
+
+        wGround = Matrix3D().also { it.makeOzMatrix(astro.si2000(point.t)) } * wGround
+        wGround = absToOrbBook * wGround
+        wGround = orbBookToOrbMatrix() * wGround
+        wGround = connectedToOrbit.transpose() * wGround
+        wGround = rotationZ(-lineAngle(nlv)) * wGround
+
+        return wGround / range
     }
 
     protected fun routeNormalInGreenwich(b: Double, l: Double, h: Double, azimuth: Double): Vector3D {

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

@@ -1,8 +1,10 @@
 package space.nstart.pcp.angularmotion
 
 import ballistics.orbitalPoints.timeStepper.AbstractStepper
+import ballistics.types.BLHPoint
 import ballistics.types.EarthType
 import ballistics.types.Orientation
+import ballistics.utils.math.Vector3D
 
 /**
  * Режим постоянной ориентации, порт ConstOrientAISTPUUD без AIST-префикса.
@@ -36,29 +38,61 @@ class ConstOrientPUUD(
         var t = tn
         var elapsed = 0.0
         var previous: AngularMotionPoint? = null
-        var b = id.b
+
-        var l = id.l
+        // В режиме ConstOrient ЦЛВ не должна сопровождать одну и ту же земную точку.
-        var h = id.h
+        // На старте фиксируем направление ЦЛВ в ОСК, а далее на каждом шаге заново
+        // пересекаем эту фиксированную ЦЛВ с Землей. Важно, чтобы первая точка маршрута
+        // тоже была получена через такое пересечение: иначе первый шаг сшивает заданную
+        // исходную ЦЛМ с расчетной трассой постоянной ЦЛВ и дает искусственный скачок рысканья.
+        val fixedClvOrientation = orientOnPoint(id.nlv, tn, id.b, id.l, id.h)
 
         while (elapsed <= duration + 2.0 * step + EPS) {
-            val routeDirection = routeDirectionInOrbitalFrame(id.copy(b = b, l = l, h = h), t)
+            val currentGround = pointOnFixedClv(t, id.nlv, fixedClvOrientation)
-            val orientation = orientOnPoint(id.nlv, t, b, l, h, routeDirection)
+                ?: BLHPoint(id.b, id.l, id.h)
+            val routeDirectionGsk = fixedClvGroundDirection(t, step, id.nlv, fixedClvOrientation, currentGround)
+            val orientation = orientOnPoint(
+                id.nlv,
+                t,
+                currentGround.lat,
+                currentGround.long,
+                currentGround.h,
+                routeDirectionGsk,
+            )
             val point = buildPoint(t, id.nlv, orientation, previous)
             result += point
             previous = point
 
-            // В исходном ConstOrientAISTPUUD точка маршрута пересчитывается по новой
-            // точке пересечения ЛВ с Землей, что удерживает ЦЛВ в орбитальной СК.
-            point.groundPoint?.let {
-                b = it.lat
-                l = it.long
-                h = id.h
-            }
-
             elapsed += step
             t += step
         }
 
         return if (result.size > 2) result.dropLast(2) else result
     }
+
+    private fun pointOnFixedClv(t: Double, nlv: Int, fixedClvOrientation: Orientation): BLHPoint? =
+        pointOnEarth(pointAt(t), nlv, fixedClvOrientation)
+
+    private fun fixedClvGroundDirection(
+        t: Double,
+        step: Double,
+        nlv: Int,
+        fixedClvOrientation: Orientation,
+        currentGround: BLHPoint,
+    ): Vector3D {
+        val previousGround = pointOnFixedClv(t - step, nlv, fixedClvOrientation)
+        val nextGround = pointOnFixedClv(t + step, nlv, fixedClvOrientation)
+
+        return when {
+            previousGround != null && nextGround != null -> groundDirection(previousGround, nextGround)
+            nextGround != null -> groundDirection(currentGround, nextGround)
+            previousGround != null -> groundDirection(previousGround, currentGround)
+            else -> Vector3D()
+        }
+    }
+
+    private fun groundDirection(from: BLHPoint, to: BLHPoint): Vector3D {
+        val first = astro.earth.blh2xyz(from.lat, from.long, from.h)
+        val second = astro.earth.blh2xyz(to.lat, to.long, to.h)
+        return second - first
+    }
 }

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

@@ -144,9 +144,11 @@ internal fun conToOpticMatrix(): Matrix3D = Matrix3D(
 )
 
 internal fun lineOfSightVectorInConnected(lineAngle: Double): Vector3D {
-    // Центральная ЛВ в существующей ballistics-lib соответствует (0, 1, 0).
+    // Центральная линия визирования направлена из центра КА к Земле.
-    // Отклонение ЛВ переносится как поворот вокруг OZ связанной СК.
+    // В программной/связанной СК при нулевых углах +OY направлена от Земли,
-    return Vector3D(-sin(lineAngle), cos(lineAngle), 0.0).normSafe()
+    // поэтому ЦЛВ соответствует вектору (0, -1, 0). Отклонения боковых ЛВ
+    // задаются поворотом вокруг OZ связанной СК, как в OrbitalMotion::conToCLV.
+    return Vector3D(sin(lineAngle), -cos(lineAngle), 0.0).normSafe()
 }
 
 internal fun quaternionFromMatrix(m: Matrix3D): Quaternion3D = Quaternion3D().also { it.fromMatrixStanley(m) }.normalized()

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

@@ -116,7 +116,7 @@ class AngularMotionCalculatorSmokeTest {
         assert(stepper != null)
         if (stepper == null)
             return
-        val calculator = AzimuthPUUD(stepper, EarthType.PZ90d02)
+        val calculator = ConstOrientPUUD(stepper, EarthType.PZ90d02)
         val id = SurveyId(
             oep = listOf(false, false, false, true),
             nlv = 5,

libs/ballistics-lib/src/main/kotlin/ballistics/mpl/OrientOnPointCalculator.kt

@@ -17,7 +17,7 @@ import kotlin.math.sign
 import kotlin.math.sin
 import kotlin.math.sqrt
 
-internal class OrientOnPointCalculator(val earthType: EarthType, val wcs: WorkCSType, val tangType: TangageType) {
+class OrientOnPointCalculator(val earthType: EarthType, val wcs: WorkCSType, val tangType: TangageType) {
     var astro = AstronomerJ2000(earthType)
 
     fun pointInWCS(