Контур по параметрам съемочного устройства

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

@@ -20,28 +20,28 @@ class SurveyContourCalculator(
 ) {
     private val pointOnEarthCalculator = PointOnEarthCalculator(earthType, wcs)
 
-    fun calculate(result: AngularMotionResult, captureAngleDeg: Double): String {
-        require(captureAngleDeg > 0.0 && captureAngleDeg.isFinite()) {
-            "Угол захвата аппаратуры должен быть положительным"
-        }
+    fun calculate(result: AngularMotionResult, y: Double,z: Double,focus: Double): String {
+
 
         val sourcePoints = result.points
         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 rightPoint = pointOnEarthCalculator.pointOnEarth(
                 point.orbitalPoint,
-                point.orientation.withKren(point.orientation.kren + captureAngleRad),
+                point.orientation,
+                z, y, focus
+
             )
             val leftPoint = pointOnEarthCalculator.pointOnEarth(
                 point.orbitalPoint,
-                point.orientation.withKren(point.orientation.kren - captureAngleRad),
+                point.orientation,
+                -z, y, focus
             )
 
             if (rightPoint != null && leftPoint != null) {

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

@@ -13,6 +13,7 @@ import java.time.LocalDateTime
 import java.time.format.DateTimeFormatter
 import java.util.Locale
 import kotlin.math.PI
+import kotlin.math.atan
 import kotlin.math.cos
 import kotlin.math.sin
 import kotlin.test.Test
@@ -73,7 +74,8 @@ class AngularMotionCalculatorSmokeTest {
             )
         )
 
-        val wkt = SurveyContourCalculator().calculate(result, captureAngleDeg = 1.5)
+        val wkt = SurveyContourCalculator().calculate(result, 0.0, AngularMotionConfig().focus * atan(1.5 * PI / 180),
+            AngularMotionConfig().focus)
         val coordinates = wkt
             .removePrefix("POLYGON ((")
             .removeSuffix("))")

libs/ballistics-lib/src/main/kotlin/ballistics/flightLine/PointOnEarthCalculator.kt

@@ -17,6 +17,76 @@ import kotlin.math.sqrt
 class PointOnEarthCalculator(val earthType: EarthType, private val wcs: WorkCSType) {
     private var astro = AstronomerJ2000(earthType)
 
+
+
+    fun pointOnEarth(
+        point: OrbitalPoint,
+        orientation: Orientation,
+        x : Double,
+        y : Double,
+        focus : Double
+
+    ): THBLPoint? {
+        val krenMax = asin((astro.earth.polarRadius - 50000.0) / point.r.module())
+        if (orientation.kren > krenMax) {
+            orientation.kren = krenMax
+        }
+        if (orientation.kren < -krenMax) {
+            orientation.kren = -krenMax
+        }
+
+        val rabs: Vector3D
+        val vabs: Vector3D
+
+        if (wcs == WorkCSType.WCSPath) {
+            rabs = point.r
+            vabs = point.v
+        } else {
+            val ask = astro.grinvToASK(point)
+
+            rabs = ask.r
+            vabs = ask.v
+        }
+        val dd: Vector3D
+        val c = Matrix3D()
+        c.second = rabs.basis()
+        c.third = (vabs.rem(rabs)).basis()
+        c.first = c.second.rem(c.third)
+        var ct = c.transpose()
+        val aa =
+            Matrix3D(
+                Vector3D(
+                    cos(orientation.tang) * cos(orientation.risk) - sin(orientation.tang) * sin(orientation.kren) * sin(orientation.risk),
+                    -sin(orientation.tang) * cos(orientation.kren),
+                    cos(orientation.tang) * sin(orientation.risk) + sin(orientation.tang) * sin(orientation.kren) * cos(orientation.risk),
+                ),
+                Vector3D(
+                    sin(orientation.tang) * cos(orientation.risk) + cos(orientation.tang) * sin(orientation.kren) * sin(orientation.risk),
+                    cos(orientation.tang) * cos(orientation.kren),
+                    sin(orientation.tang) * sin(orientation.risk) - cos(orientation.tang) * sin(orientation.kren) * cos(orientation.risk),
+                ),
+                Vector3D(
+                    -cos(orientation.kren) * sin(orientation.risk),
+                    sin(orientation.kren),
+                    cos(orientation.kren) * cos(orientation.risk),
+                ),
+            )
+        if (wcs == WorkCSType.WCSOrbit) {
+            val g = Matrix3D()
+            g.makeOzMatrix(astro.si2000(point.t))
+            ct = g.transpose() * ct
+        }
+
+        val k = ct * aa
+
+        val d = Vector3D(y / 1000.0, focus / 1000.0, x / 1000).basis()
+
+        dd = k * d
+
+        return earthIntersection(point.t, point.r, dd)
+    }
+
+
     fun pointOnEarth(
         point: OrbitalPoint,
         orientation: Orientation,