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

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
@@ -11,6 +11,7 @@ data class AngularMotionCalculationRequestDTO(
    val longitudeDeg: Double,
    val heightM: Double = 0.0,
    val durationSec: Double,
+    val captureAngleDeg: Double = 1.5,
    val leadAngleDeg: Double = 0.0,
    val azimuthDeg: Double = 0.0,
    val sdi: Double? = null,
@@ -24,6 +24,7 @@ class AngularMotionCalculationRequestDTOJacksonTest {
              "longitudeDeg": 37.61,
              "heightM": 0,
              "durationSec": 60,
+              "captureAngleDeg": 1.5,
              "leadAngleDeg": 0,
              "azimuthDeg": 0,
              "pointInCenter": false,
@@ -39,6 +40,7 @@ class AngularMotionCalculationRequestDTOJacksonTest {
        assertEquals(AngularMotionModeDTO.CONST_ORIENT, request.mode)
        assertEquals(55.75, request.latitudeDeg)
        assertEquals(37.61, request.longitudeDeg)
+        assertEquals(1.5, request.captureAngleDeg)
        assertFalse(request.pointInCenter)
    }
 }
@@ -28,6 +28,7 @@ data class AngularMotionCalculationResultDTO(
    val satelliteId: Long,
    val approximateTime: LocalDateTime,
    val startTime: LocalDateTime,
+    val contourWkt: String?,
    val points: List<AngularMotionPointDTO>,
 )

@@ -10,6 +10,7 @@ import org.springframework.stereotype.Service
 import space.nstart.pcp.angularmotion.AngularMotionCalculatorFactory
 import space.nstart.pcp.angularmotion.AngularMotionConfig
 import space.nstart.pcp.angularmotion.AngularMotionMode
+import space.nstart.pcp.angularmotion.SurveyContourCalculator
 import space.nstart.pcp.angularmotion.SurveyId
 import space.nstart.pcp.pcp_request_service.configuration.CustomValidationException
 import space.nstart.pcp.pcp_request_service.service.SatelliteCatalogClient
@@ -91,12 +92,14 @@ class AngularMotionService(
        } catch (error: RuntimeException) {
            throw CustomValidationException(error.message ?: "Ошибка расчета ПУУД")
        }
+        val contourWkt = SurveyContourCalculator(EarthType.PZ90d02).calculate(result, request.captureAngleDeg)

        return AngularMotionCalculationResultDTO(
            mode = result.mode,
            satelliteId = request.satelliteId,
            approximateTime = request.approximateTime,
            startTime = toDateTime(result.startTime),
+            contourWkt = contourWkt,
            points = result.points.map { point ->
                AngularMotionPointDTO(
                    time = toDateTime(point.t),
@@ -122,6 +125,9 @@ class AngularMotionService(
        if (request.durationSec <= 0.0) {
            throw CustomValidationException("durationSec must be positive")
        }
+        if (request.captureAngleDeg <= 0.0 || !request.captureAngleDeg.isFinite()) {
+            throw CustomValidationException("captureAngleDeg must be positive")
+        }
        if (request.latitudeDeg !in -90.0..90.0) {
            throw CustomValidationException("latitudeDeg must be in [-90; 90]")
        }
@@ -284,6 +284,54 @@
        });
    }

+    function polygonCoordinatesFromWkt(wkt) {
+        const textValue = text(wkt).trim();
+        const match = textValue.match(/^POLYGON\s*\(\((.*)\)\)$/i);
+        if (!match) return [];
+
+        return match[1].split(',')
+            .map(item => item.trim().split(/\s+/).map(Number))
+            .filter(pair => pair.length >= 2 && Number.isFinite(pair[0]) && Number.isFinite(pair[1]))
+            .map(pair => ({ lon: pair[0], lat: pair[1] }));
+    }
+
+    function surveyContourBounds() {
+        const coordinates = polygonCoordinatesFromWkt(state.result?.contourWkt);
+        return {
+            lats: coordinates.map(point => point.lat),
+            lons: coordinates.map(point => point.lon),
+        };
+    }
+
+    function addSurveyContour() {
+        if (!state.mapDataSource || !state.result?.contourWkt) return;
+        const coordinates = polygonCoordinatesFromWkt(state.result.contourWkt);
+        if (coordinates.length < 4) return;
+
+        const degrees = [];
+        coordinates.forEach(point => {
+            degrees.push(point.lon, point.lat);
+        });
+        const positions = Cesium.Cartesian3.fromDegreesArray(degrees);
+        state.mapDataSource.entities.add({
+            name: 'Контур съемки',
+            polygon: {
+                hierarchy: new Cesium.PolygonHierarchy(positions.slice(0, -1)),
+                material: Cesium.Color.ORANGE.withAlpha(0.22),
+                heightReference: Cesium.HeightReference.CLAMP_TO_GROUND,
+            },
+        });
+        state.mapDataSource.entities.add({
+            name: 'Граница контура съемки',
+            polyline: {
+                positions,
+                width: 2,
+                material: Cesium.Color.ORANGE,
+                clampToGround: true,
+            },
+        });
+    }
+
    function addTarget() {
        if (!state.mapDataSource) return;
        const lat = Number(el('angular-motion-lat').value);
@@ -327,6 +375,9 @@
                if (Number.isFinite(value)) lons.push(value);
            });
        });
+        const contourBounds = surveyContourBounds();
+        lats.push(...contourBounds.lats);
+        lons.push(...contourBounds.lons);
        if (lats.length === 0 || lons.length === 0) return;
        const minLat = Math.max(-89.0, Math.min(...lats) - 1.0);
        const maxLat = Math.min(89.0, Math.max(...lats) + 1.0);
@@ -346,6 +397,7 @@
        addPolyline(points, 'leftLatitudeDeg', 'leftLongitudeDeg', Cesium.Color.CORNFLOWERBLUE.withAlpha(0.85), 2, 'Левая граница полосы');
        addPolyline(points, 'rightLatitudeDeg', 'rightLongitudeDeg', Cesium.Color.CORNFLOWERBLUE.withAlpha(0.85), 2, 'Правая граница полосы');
        addPolyline(points, 'latitudeDeg', 'longitudeDeg', Cesium.Color.YELLOW, 3, 'Трасса полета');
+        addSurveyContour();
        addTarget();
        if (fit) fitMapToData();
        state.viewer.scene.requestRender();
@@ -377,6 +429,7 @@
            longitudeDeg: Number(el('angular-motion-lon').value),
            heightM: Number(el('angular-motion-height').value || 0),
            durationSec: Number(el('angular-motion-duration').value),
+            captureAngleDeg: Number(el('angular-motion-capture-angle').value || 1.5),
            leadAngleDeg: Number(el('angular-motion-lead-angle').value || 0),
            azimuthDeg: Number(el('angular-motion-azimuth').value || 0),
            pointInCenter: false,
@@ -394,7 +447,7 @@
        state.result = result;
        const rows = Array.isArray(result?.points) ? result.points : [];
        el('angular-motion-result-meta').textContent = rows.length > 0
-            ? `Старт режима: ${formatDateTime(result.startTime)}. Точек: ${rows.length}.`
+            ? `Старт режима: ${formatDateTime(result.startTime)}. Точек: ${rows.length}. Контур: ${result?.contourWkt ? 'построен' : 'нет'}.`
            : 'Расчет не вернул точек.';
        el('angular-motion-export-csv').disabled = rows.length === 0;
        const body = el('angular-motion-result-body');
@@ -420,6 +473,7 @@
                <td>${escapeHtml(formatNumber(point.sdi, 4))}</td>
            </tr>
        `).join('');
+        drawMap(true);
    }

    async function calculate(event) {
@@ -78,10 +78,17 @@
                        </div>

                        <div class="row g-2 mb-3">
+                            <div class="col-12 col-md-6">
+                                <label for="angular-motion-capture-angle" class="form-label">Угол захвата, град</label>
+                                <input id="angular-motion-capture-angle" type="number" min="0.001" step="0.001" class="form-control" value="1.5" required>
+                            </div>
                            <div class="col-12 col-md-6">
                                <label for="angular-motion-lead-angle" class="form-label">Упреждающий угол, град</label>
                                <input id="angular-motion-lead-angle" type="number" step="0.001" class="form-control" value="0">
                            </div>
+                        </div>
+
+                        <div class="row g-2 mb-3">
                            <div class="col-12 col-md-6 angular-motion-azimuth-field">
                                <label for="angular-motion-azimuth" class="form-label">Азимут, град</label>
                                <input id="angular-motion-azimuth" type="number" step="0.001" class="form-control" value="0">