fix в задании азимута

2026-06-15 15:11:20 +03:00
parent 29b8206867
commit 60ddec0928
9 changed files with 134 additions and 159 deletions
@@ -279,7 +279,7 @@ abstract class AbstractPuudCalculator(
        return orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v) * routeAbs
    }

-    private fun ellipsoidNormalInGreenwich(b: Double, l: Double): Vector3D =
+    protected fun ellipsoidNormalInGreenwich(b: Double, l: Double): Vector3D =
        Vector3D(cos(b) * cos(l), cos(b) * sin(l), sin(b)).normSafe()

    private fun eastDirectionInGreenwich(l: Double): Vector3D =
@@ -8,6 +8,7 @@ import ballistics.utils.math.Quaternion3D
 import ballistics.utils.math.Vector3D
 import kotlin.math.PI
 import kotlin.math.abs
+import kotlin.math.atan2
 import kotlin.math.cos
 import kotlin.math.sin
 import kotlin.math.sqrt
@@ -60,7 +61,13 @@ open class AzimuthPUUD(
            time = tn,
            reverse = initialSdi < 0.0,
        )
-        var liv = initialLiv
+        var liv = alignVisirQuaternionToRoute(
+            liv = initialLiv,
+            time = tn,
+            routeNormalGsk = routeNormalGsk,
+            rAbs = firstAbs.r,
+            reverse = initialSdi < 0.0,
+        )

        val result = mutableListOf<AngularMotionPoint>()
        var t = tn
@@ -70,9 +77,16 @@ 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)
+            liv = alignVisirQuaternionToRoute(
+                liv = liv,
+                time = t,
+                routeNormalGsk = routeNormalGsk,
+                rAbs = ask.r,
+                reverse = sdiForTime < 0.0,
+            )
+            val di = slantRangeFromQuaternion(liv, ask.r)
+            val omegaVisir = ownCornerSpeed(t, liv, di, ask.r, ask.v, sdiForTime, routeNormalGsk)
            val orientation = orientationFromVisirQuaternion(orbital, liv)
            val point = buildIntegratedPoint(t, orientation, omegaVisir, previous, sickle)
            result += point
@@ -84,8 +98,17 @@ open class AzimuthPUUD(
                    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)
+                    ownCornerSpeed(time, q, pDi, pAsk.r, pAsk.v, pSdi, routeNormalGsk)
                }
+                val nextOrbital = pointAt(t + step)
+                val nextAsk = astro.grinvToASK(nextOrbital)
+                liv = alignVisirQuaternionToRoute(
+                    liv = liv,
+                    time = t + step,
+                    routeNormalGsk = routeNormalGsk,
+                    rAbs = nextAsk.r,
+                    reverse = sdiAt(id.sdi, elapsed + step) < 0.0,
+                )
            }
            elapsed += step
            t += step
@@ -101,6 +124,7 @@ open class AzimuthPUUD(
        rAbs: Vector3D,
        vAbs: Vector3D,
        sdi: Double,
+        routeNormalGsk: Vector3D?,
    ): Vector3D {
        val omegaEarth = Vector3D(0.0, 0.0, astro.earth.wEarth)
        val relVelocityAbs = vAbs - omegaEarth.rem(rAbs)
@@ -156,11 +180,10 @@ open class AzimuthPUUD(
        val routeNormalAbs = astro.grinvToASK(routeNormalGsk, time).normSafe()
        val routeDirectionAbs = astro.grinvToASK(routeDirectionGsk, time).normSafe()

-        // Ось движения изображения в визирной СК должна быть связана с касательной
-        // центральной линии маршрута, а не с хордой двух земных радиус-векторов.
-        // Проекция азимутальной касательной в фокальную плоскость соответствует
-        // критериям Wz/D = 0 и Wx/D = const: поперечная составляющая обнуляется,
-        // а продольная ось направлена по маршруту.
+        // Азимут задаёт именно направление движения точки визирования по поверхности
+        // Земли: 0° — на север, 90° — на восток. Поэтому ось сканирования визирной
+        // СК строится по проекции азимутальной касательной на фокальную плоскость,
+        // без смены знака. Иначе азимут 0° фактически превращается в 180°.
        var routeAxis = (routeDirectionAbs - line * (routeDirectionAbs * line)).normSafe()
        if (routeAxis.module() < EPS) {
            routeAxis = routeNormalAbs.rem(line).normSafe()
@@ -182,6 +205,60 @@ open class AzimuthPUUD(
        return quaternionFromMatrix(m).inverse().normalized()
    }

+    /**
+     * Доворачивает визирную систему вокруг текущей ЦЛВ так, чтобы ее ось
+     * сканирования была направлена по заданному азимутальному маршруту на Земле.
+     *
+     * В старой реализации азимут участвовал только в начальной ориентации. После
+     * интегрирования кватерниона оси фокальной плоскости могли дрейфовать вокруг
+     * ЦЛВ, поэтому симметричные азимуты, например 20° и 340°, давали одинаковую
+     * программу. Здесь азимутальный маршрут используется на каждом шаге: в текущей
+     * точке визирования строится касательная к плоскости маршрута и визирная СК
+     * поворачивается вокруг ЦЛВ до совпадения с этой касательной.
+     */
+    private fun alignVisirQuaternionToRoute(
+        liv: Quaternion3D,
+        time: Double,
+        routeNormalGsk: Vector3D,
+        rAbs: Vector3D,
+        reverse: Boolean,
+    ): Quaternion3D {
+        val lineAbs = (liv * Vector3D(1.0, 0.0, 0.0)).normSafe()
+        if (lineAbs.module() < EPS) return liv
+
+        val di = slantRangeFromQuaternion(liv, rAbs)
+        if (di.module() < EPS) return liv
+
+        val groundAbs = rAbs + di
+        val groundGsk = astro.askToGrinvich(groundAbs, time)
+        val groundBlh = astro.earth.xyz2blh(groundGsk)
+        val normalGsk = ellipsoidNormalInGreenwich(groundBlh.lat, groundBlh.long)
+        // Для плоскости маршрута с нормалью N_route и текущей нормали к ОЗЭ U
+        // касательная в направлении заданного азимута равна U x N_route. Обратный
+        // порядок N_route x U разворачивает маршрут: 0° ведёт на юг, 180° — на север.
+        var desiredGsk = normalGsk.rem(routeNormalGsk).normSafe()
+        if (desiredGsk.module() < EPS) return liv
+        if (reverse) {
+            desiredGsk = desiredGsk * -1.0
+        }
+
+        val desiredAbsRaw = astro.grinvToASK(desiredGsk, time).normSafe()
+        val desiredAbs = (desiredAbsRaw - lineAbs * (desiredAbsRaw * lineAbs)).normSafe()
+        if (desiredAbs.module() < EPS) return liv
+
+        val currentAxisRaw = (liv * Vector3D(0.0, 1.0, 0.0)).normSafe()
+        val currentAxis = (currentAxisRaw - lineAbs * (currentAxisRaw * lineAbs)).normSafe()
+        if (currentAxis.module() < EPS) return liv
+
+        val cosAngle = (currentAxis * desiredAbs).coerceIn(-1.0, 1.0)
+        val sinAngle = lineAbs * currentAxis.rem(desiredAbs)
+        val angle = atan2(sinAngle, cosAngle)
+        if (abs(angle) < 1.0e-13) return liv
+
+        val correction = Quaternion3D().also { it.makeQuat(angle, lineAbs) }
+        return (correction * liv).normalized()
+    }
+
    protected fun slantRangeFromQuaternion(liv: Quaternion3D, rAbs: Vector3D): Vector3D {
        val lineAbs = (liv * Vector3D(1.0, 0.0, 0.0)).normSafe()
        val a = astro.earth.ekvRadius
@@ -41,6 +41,7 @@ class SmoothSDIPUUD(
        rAbs: Vector3D,
        vAbs: Vector3D,
        sdi: Double,
+        routeNormalGsk: Vector3D?,
    ): Vector3D {
        val omegaEarth = Vector3D(0.0, 0.0, astro.earth.wEarth)
        val relVelocityAbs = vAbs - omegaEarth.rem(rAbs)
@@ -54,7 +55,7 @@ class SmoothSDIPUUD(
        val miv = liv.matrix().transpose()
        val currentAbs = rAbs + di
        val currentGsk = astro.askToGrinvich(currentAbs, time)
-        val mih = horizontalGeodesicToAbs(currentGsk, time)
+        val mih = horizontalGeodesicToAbs(currentGsk, time, routeNormalGsk)
        val chv = miv * mih.transpose()
        val perspective = visToOptic * chv
        val denom = perspective.first.x.coerceAwayFromZero()
@@ -68,15 +69,27 @@ class SmoothSDIPUUD(
        return omegaInVisir
    }

-    private fun horizontalGeodesicToAbs(pointGsk: Vector3D, time: Double): Matrix3D {
+    private fun horizontalGeodesicToAbs(
+        pointGsk: Vector3D,
+        time: Double,
+        routeNormalGsk: Vector3D?,
+    ): Matrix3D {
        val blh = astro.earth.xyz2blh(pointGsk)
-        val upGsk = pointGsk.normSafe()
-        val eastGsk = Vector3D(-kotlin.math.sin(blh.long), kotlin.math.cos(blh.long), 0.0).normSafe()
-        val northGsk = upGsk.rem(eastGsk).normSafe()
+        val upGsk = ellipsoidNormalInGreenwich(blh.lat, blh.long)
+        val alongGsk = routeNormalGsk
+            ?.let { upGsk.rem(it) }
+            ?.normSafe()
+            ?.takeIf { it.module() >= EPS }
+            ?: Vector3D(
+                -kotlin.math.sin(blh.lat) * kotlin.math.cos(blh.long),
+                -kotlin.math.sin(blh.lat) * kotlin.math.sin(blh.long),
+                kotlin.math.cos(blh.lat),
+            ).normSafe()
+        val crossGsk = upGsk.rem(alongGsk).normSafe()
        val gskToAbs = Matrix3D().also { it.makeOzMatrix(astro.si2000(time)) }
        return Matrix3D(
-            gskToAbs * northGsk,
-            gskToAbs * eastGsk,
+            gskToAbs * alongGsk,
+            gskToAbs * crossGsk,
            gskToAbs * upGsk,
        )
    }
@@ -1,40 +1,31 @@
 package space.nstart.pcp.angularmotion

-import ballistics.mpl.OrientOnPointCalculator
-import ballistics.types.BLHPoint
+import ballistics.flightLine.PointOnEarthCalculator
 import ballistics.types.EarthType
-import ballistics.types.OrbitalPoint
-import ballistics.types.TangageType
+import ballistics.types.Orientation
+import ballistics.types.THBLPoint
 import ballistics.types.WorkCSType
-import ballistics.utils.earth.getEarth
 import java.util.Locale
 import kotlin.math.PI
-import kotlin.math.abs
-import kotlin.math.cos
-import kotlin.math.max
-import kotlin.math.sin
-import kotlin.math.sqrt

 /**
 * Строит WKT-контур полосы съемки по результату ПУУД.
 *
- * [captureAngleDeg] трактуется как полуширина полосы: правая/левая граница соответствуют
- * ориентациям на точки с креном `centerKren + capture` и `centerKren - capture`.
+ * [captureAngleDeg] трактуется как полуширина полосы. Правая/левая граница строятся
+ * прямым пересечением визирного луча с Землей через [PointOnEarthCalculator].
 */
 class SurveyContourCalculator(
    earthType: EarthType = EarthType.PZ90d02,
    wcs: WorkCSType = WorkCSType.WCSOrbit,
-    tangageType: TangageType = TangageType.TTProactive,
 ) {
-    private val earth = getEarth(earthType)
-    private val orientCalculator = OrientOnPointCalculator(earthType, wcs, tangageType)
+    private val pointOnEarthCalculator = PointOnEarthCalculator(earthType, wcs)

    fun calculate(result: AngularMotionResult, captureAngleDeg: Double): String {
        require(captureAngleDeg > 0.0 && captureAngleDeg.isFinite()) {
            "Угол захвата аппаратуры должен быть положительным"
        }

-        val sourcePoints = result.points.filter { it.groundPoint != null }
+        val sourcePoints = result.points
        require(sourcePoints.size >= MIN_CONTOUR_POINTS) {
            "Для построения контура съемки требуется не менее $MIN_CONTOUR_POINTS точек ПУУД"
        }
@@ -44,9 +35,22 @@ class SurveyContourCalculator(
        val left = ArrayList<SurveyContourPoint>(sourcePoints.size)

        sourcePoints.forEach { point ->
-            val centerKren = orientationKren(point.orbitalPoint, point.groundPoint!!)
-            right += boundaryPoint(point.orbitalPoint, point.groundPoint!!, centerKren + captureAngleRad)
-            left += boundaryPoint(point.orbitalPoint, point.groundPoint!!, centerKren - captureAngleRad)
+            val rightPoint = pointOnEarthCalculator.pointOnEarth(
+                point.orbitalPoint,
+                point.orientation.withKren(point.orientation.kren + captureAngleRad),
+            )
+            val leftPoint = pointOnEarthCalculator.pointOnEarth(
+                point.orbitalPoint,
+                point.orientation.withKren(point.orientation.kren - captureAngleRad),
+            )
+
+            if (rightPoint != null && leftPoint != null) {
+                right += rightPoint.toContourPoint()
+                left += leftPoint.toContourPoint()
+            }
+        }
+        require(right.size >= MIN_CONTOUR_POINTS && left.size >= MIN_CONTOUR_POINTS) {
+            "Для построения контура съемки не удалось рассчитать границы полосы"
        }

        val shell = ArrayList<SurveyContourPoint>(right.size + left.size + 1)
@@ -58,102 +62,6 @@ class SurveyContourCalculator(
        return toWkt(shell)
    }

-    private fun boundaryPoint(orbitalPoint: OrbitalPoint, center: BLHPoint, targetKren: Double): SurveyContourPoint {
-        val centerKren = orientationKren(orbitalPoint, center)
-        val targetDelta = angleDelta(targetKren, centerKren)
-        if (abs(targetDelta) < EPS) {
-            return center.toContourPoint()
-        }
-
-        val direction = krenGradientDirection(orbitalPoint, center, centerKren, targetDelta)
-        var high = initialSearchDistanceMeters(orbitalPoint, center, centerKren, targetDelta, direction)
-        var highDelta = projectedKrenDelta(orbitalPoint, center, centerKren, direction, high)
-
-        while (sameDirection(targetDelta, highDelta) && abs(highDelta) < abs(targetDelta) && high < MAX_SEARCH_DISTANCE_M) {
-            high = (high * 2.0).coerceAtMost(MAX_SEARCH_DISTANCE_M)
-            highDelta = projectedKrenDelta(orbitalPoint, center, centerKren, direction, high)
-        }
-
-        var low = 0.0
-        repeat(BINARY_SEARCH_ITERATIONS) {
-            val mid = (low + high) / 2.0
-            val midDelta = projectedKrenDelta(orbitalPoint, center, centerKren, direction, mid)
-            if (sameDirection(targetDelta, midDelta) && abs(midDelta) < abs(targetDelta)) {
-                low = mid
-            } else {
-                high = mid
-            }
-        }
-
-        return shift(center, direction, high).toContourPoint()
-    }
-
-    private fun krenGradientDirection(
-        orbitalPoint: OrbitalPoint,
-        center: BLHPoint,
-        centerKren: Double,
-        targetDelta: Double,
-    ): GroundDirection {
-        val eastKren = orientationKren(orbitalPoint, shift(center, GroundDirection(1.0, 0.0), GRADIENT_STEP_M))
-        val northKren = orientationKren(orbitalPoint, shift(center, GroundDirection(0.0, 1.0), GRADIENT_STEP_M))
-        val eastDelta = angleDelta(eastKren, centerKren)
-        val northDelta = angleDelta(northKren, centerKren)
-        val norm = sqrt(eastDelta * eastDelta + northDelta * northDelta)
-        val sign = if (targetDelta >= 0.0) 1.0 else -1.0
-
-        return if (norm < EPS) {
-            GroundDirection(sign, 0.0)
-        } else {
-            GroundDirection(sign * eastDelta / norm, sign * northDelta / norm)
-        }
-    }
-
-    private fun initialSearchDistanceMeters(
-        orbitalPoint: OrbitalPoint,
-        center: BLHPoint,
-        centerKren: Double,
-        targetDelta: Double,
-        direction: GroundDirection,
-    ): Double {
-        val deltaAtStep = abs(projectedKrenDelta(orbitalPoint, center, centerKren, direction, GRADIENT_STEP_M))
-        if (deltaAtStep < EPS) return DEFAULT_SEARCH_DISTANCE_M
-        return max(GRADIENT_STEP_M, GRADIENT_STEP_M * abs(targetDelta) / deltaAtStep * 1.25)
-            .coerceAtMost(MAX_SEARCH_DISTANCE_M)
-    }
-
-    private fun projectedKrenDelta(
-        orbitalPoint: OrbitalPoint,
-        center: BLHPoint,
-        centerKren: Double,
-        direction: GroundDirection,
-        distanceMeters: Double,
-    ): Double = angleDelta(orientationKren(orbitalPoint, shift(center, direction, distanceMeters)), centerKren)
-
-    private fun orientationKren(orbitalPoint: OrbitalPoint, point: BLHPoint): Double =
-        orientCalculator.calculateOrientOnPoint(
-            orbitalPoint,
-            earth.blh2xyz(point.lat, point.long, point.h),
-        ).kren
-
-    private fun shift(center: BLHPoint, direction: GroundDirection, distanceMeters: Double): BLHPoint {
-        val latitude = center.lat + direction.north * distanceMeters / earth.middleRadius
-        val parallelRadius = (earth.middleRadius * cos(center.lat)).coerceAwayFromZero()
-        val longitude = normalizeLongitude(center.long + direction.east * distanceMeters / parallelRadius)
-        return BLHPoint(clamp(latitude, -MAX_LATITUDE_RAD, MAX_LATITUDE_RAD), longitude, center.h)
-    }
-
-    private fun sameDirection(expected: Double, actual: Double): Boolean =
-        actual == 0.0 || expected == 0.0 || expected * actual > 0.0
-
-    private fun angleDelta(value: Double, reference: Double): Double = normalizeAngle(value - reference)
-
-    private fun clamp(value: Double, min: Double, max: Double): Double =
-        when {
-            value < min -> min
-            value > max -> max
-            else -> value
-        }
-
    private fun toWkt(points: List<SurveyContourPoint>): String {
        val coordinates = points.joinToString(", ") { point ->
            String.format(Locale.US, "%.8f %.8f", point.longitudeDeg, point.latitudeDeg)
@@ -161,25 +69,15 @@ class SurveyContourCalculator(
        return "POLYGON (($coordinates))"
    }

-    private fun BLHPoint.toContourPoint(): SurveyContourPoint =
+    private fun Orientation.withKren(kren: Double): Orientation = Orientation(tang, kren, risk)
+
+    private fun THBLPoint.toContourPoint(): SurveyContourPoint =
        SurveyContourPoint(latitudeDeg = lat.toDegrees(), longitudeDeg = long.toDegrees())

    private fun Double.toRadians(): Double = this * PI / 180.0

    private fun Double.toDegrees(): Double = this * 180.0 / PI

-    private fun normalizeLongitude(value: Double): Double {
-        var longitude = value
-        while (longitude > PI) longitude -= 2.0 * PI
-        while (longitude < -PI) longitude += 2.0 * PI
-        return longitude
-    }
-
-    private data class GroundDirection(
-        val east: Double,
-        val north: Double,
-    )
-
    private data class SurveyContourPoint(
        val latitudeDeg: Double,
        val longitudeDeg: Double,
@@ -187,10 +85,5 @@ class SurveyContourCalculator(

    private companion object {
        const val MIN_CONTOUR_POINTS = 2
-        const val GRADIENT_STEP_M = 1_000.0
-        const val DEFAULT_SEARCH_DISTANCE_M = 50_000.0
-        const val MAX_SEARCH_DISTANCE_M = 2_000_000.0
-        const val BINARY_SEARCH_ITERATIONS = 48
-        const val MAX_LATITUDE_RAD = PI / 2.0 - 1.0e-8
    }
 }
@@ -18,7 +18,6 @@ data class AngularMotionCalculationRequestDTO(
    val pointInCenter: Boolean = false,
    val focusMm: Double? = null,
    val stepPuudSec: Double? = null,
-    val stepSdiSec: Double? = null,
 )

 enum class AngularMotionModeDTO {
@@ -29,8 +29,7 @@ class AngularMotionCalculationRequestDTOJacksonTest {
              "azimuthDeg": 0,
              "pointInCenter": false,
              "focusMm": 5500,
-              "stepPuudSec": 0.125,
-              "stepSdiSec": 20
+              "stepPuudSec": 0.125
            }
            """.trimIndent(),
        )
@@ -80,7 +80,6 @@ class AngularMotionService(
        val config = AngularMotionConfig(
            focus = request.focusMm ?: defaultConfig.focus,
            stepPuud = request.stepPuudSec ?: defaultConfig.stepPuud,
-            stepSdi = request.stepSdiSec ?: defaultConfig.stepSdi,
        )
        val sourcePoints = loadOrbitalPoints(request, config)
        val stepper = RungeStepper(sourcePoints.toMutableList(), EarthType.PZ90d02)
@@ -435,7 +435,6 @@
            pointInCenter: false,
            focusMm: Number(el('angular-motion-focus').value || 5500),
            stepPuudSec: Number(el('angular-motion-step').value || 0.125),
-            stepSdiSec: Number(el('angular-motion-step-sdi').value || 20),
        };
        if (mode === 'AZIMUTH' || mode === 'SMOOTH_SDI') {
            payload.sdi = Number(el('angular-motion-sdi').value);
@@ -111,10 +111,6 @@
                                <label for="angular-motion-step" class="form-label">Шаг ПУУД, с</label>
                                <input id="angular-motion-step" type="number" min="0.001" step="0.001" class="form-control" value="0.125">
                            </div>
-                            <div class="col-12 col-md-6">
-                                <label for="angular-motion-step-sdi" class="form-label">Шаг СДИ, с</label>
-                                <input id="angular-motion-step-sdi" type="number" min="0.001" step="0.001" class="form-control" value="20">
-                            </div>
                        </div>

                        <button id="angular-motion-submit" type="submit" class="btn btn-primary w-100">Рассчитать</button>