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

2026-06-08 18:43:05 +03:00
parent b62a7cf8ef
commit 69f2d6ad4f
5 changed files with 135 additions and 39 deletions
@@ -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) {
-            val route = rotationZ(-lineAngle(nlv)) * routeDirection
-            orient.risk = normalizeAngle(atan2(route.x, route.z))
+        if (routeDirectionGsk != null && routeDirectionGsk.module() > 1.0e-8) {
+            orient.risk = riskFromGroundDirection(ka, nlv, orient, routeDirectionGsk)
        }
        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 bodyToOrbit = orientationMatrix(orientation)
-        val lineOrbit = bodyToOrbit * line
-        val lineAbs = (absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose() * lineOrbit).normSafe()
-        val ground = earthIntersection(point.r, (Matrix3D().also { it.makeOzMatrix(-astro.si2000(point.t)) } * lineAbs).normSafe())
-            ?: return Vector3D()
-        val groundGsk = astro.earth.blh2xyz(ground)
-        val groundAbs = astro.grinvToASK(groundGsk, point.t)
-        val slantAbs = groundAbs - ask.r
-        val range = slantAbs.module().coerceAtLeast(EPS)
-        val omegaEarth = Vector3D(0.0, 0.0, astro.earth.wEarth)
-        val relVelocityAbs = ask.v - omegaEarth.rem(ask.r)
-        val connectedToAbs = absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose() * orientationMatrix(orientation)
-        val relVelocityConnected = connectedToAbs.transpose() * relVelocityAbs
-        val compensation = omegaConnected.rem(line) * range
-        return (relVelocityConnected - compensation) / range
+        val lineConnected = lineOfSightVectorInConnected(lineAngle(nlv))
+        val connectedToOrbit = orientationMatrix(orientation)
+        val absToOrbBook = absToOrbBookMatrix(ask.r, ask.v)
+        val orbitBookToAbs = absToOrbBook.transpose()
+        val orbitToOrbitBook = orbBookToOrbMatrix().transpose()
+        val absToGsk = Matrix3D().also { it.makeOzMatrix(-astro.si2000(point.t)) }
+
+        val lineGsk = (absToGsk * orbitBookToAbs * orbitToOrbitBook * connectedToOrbit * lineConnected).normSafe()
+        if (lineGsk.module() < EPS) return Vector3D()
+
+        val earth = astro.earth
+        val kalpha = 1.0 - earth.alphaEllips * earth.ekvRadius / earth.ekvRadius
+        val reducedR = Vector3D(point.r.x, point.r.y, point.r.z / kalpha)
+        val reducedLine = Vector3D(lineGsk.x, lineGsk.y, lineGsk.z / kalpha)
+        val reducedProjection = reducedR * reducedLine
+        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 {
@@ -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
-        var h = id.h
+
+        // В режиме ConstOrient ЦЛВ не должна сопровождать одну и ту же земную точку.
+        // На старте фиксируем направление ЦЛВ в ОСК, а далее на каждом шаге заново
+        // пересекаем эту фиксированную ЦЛВ с Землей. Важно, чтобы первая точка маршрута
+        // тоже была получена через такое пересечение: иначе первый шаг сшивает заданную
+        // исходную ЦЛМ с расчетной трассой постоянной ЦЛВ и дает искусственный скачок рысканья.
+        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 orientation = orientOnPoint(id.nlv, t, b, l, h, routeDirection)
+            val currentGround = pointOnFixedClv(t, id.nlv, fixedClvOrientation)
+                ?: 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
+    }
 }
@@ -144,9 +144,11 @@ internal fun conToOpticMatrix(): Matrix3D = Matrix3D(
 )

 internal fun lineOfSightVectorInConnected(lineAngle: Double): Vector3D {
-    // Центральная ЛВ в существующей ballistics-lib соответствует (0, 1, 0).
-    // Отклонение ЛВ переносится как поворот вокруг OZ связанной СК.
-    return Vector3D(-sin(lineAngle), cos(lineAngle), 0.0).normSafe()
+    // Центральная линия визирования направлена из центра КА к Земле.
+    // В программной/связанной СК при нулевых углах +OY направлена от Земли,
+    // поэтому ЦЛВ соответствует вектору (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()
@@ -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,