Контур для ПУУД

2026-06-15 12:49:45 +03:00
parent 7fdac8c71e
commit 29b8206867
8 changed files with 291 additions and 1 deletions
@@ -0,0 +1,196 @@
+package space.nstart.pcp.angularmotion
+
+import ballistics.mpl.OrientOnPointCalculator
+import ballistics.types.BLHPoint
+import ballistics.types.EarthType
+import ballistics.types.OrbitalPoint
+import ballistics.types.TangageType
+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`.
+ */
+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)
+
+    fun calculate(result: AngularMotionResult, captureAngleDeg: Double): String {
+        require(captureAngleDeg > 0.0 && captureAngleDeg.isFinite()) {
+            "Угол захвата аппаратуры должен быть положительным"
+        }
+
+        val sourcePoints = result.points.filter { it.groundPoint != null }
+        require(sourcePoints.size >= MIN_CONTOUR_POINTS) {
+            "Для построения контура съемки требуется не менее $MIN_CONTOUR_POINTS точек ПУУД"
+        }
+
+        val captureAngleRad = captureAngleDeg.toRadians()
+        val right = ArrayList<SurveyContourPoint>(sourcePoints.size)
+        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 shell = ArrayList<SurveyContourPoint>(right.size + left.size + 1)
+        shell += right
+        for (index in left.indices.reversed()) {
+            shell += left[index]
+        }
+        shell += shell.first()
+        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)
+        }
+        return "POLYGON (($coordinates))"
+    }
+
+    private fun BLHPoint.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,
+    )
+
+    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
+    }
+}
@@ -61,6 +61,29 @@ class AngularMotionCalculatorSmokeTest {
        assertTrue(result.points.isNotEmpty())
    }

+    @Test
+    fun `survey contour calculator returns closed wkt polygon`() {
+        val result = ConstOrientPUUD(CircularStepper()).calculate(
+            SurveyId(
+                t = 1000.0,
+                b = 0.1,
+                l = 0.2,
+                h = 0.0,
+                duration = 1.0,
+            )
+        )
+
+        val wkt = SurveyContourCalculator().calculate(result, captureAngleDeg = 1.5)
+        val coordinates = wkt
+            .removePrefix("POLYGON ((")
+            .removeSuffix("))")
+            .split(", ")
+
+        assertTrue(wkt.startsWith("POLYGON (("))
+        assertTrue(coordinates.size >= result.points.size * 2 + 1)
+        assertEquals(coordinates.first(), coordinates.last())
+    }
+
    private class CircularStepper : AbstractStepper {
        private val radius = 7_000_000.0
        private val omega = 0.001