diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SurveyContourCalculator.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SurveyContourCalculator.kt index 7c5ec9d..479aea6 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SurveyContourCalculator.kt +++ b/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(sourcePoints.size) val left = ArrayList(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) { diff --git a/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt b/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt index 2461e92..7ceca2a 100644 --- a/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt +++ b/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("))") diff --git a/libs/ballistics-lib/src/main/kotlin/ballistics/flightLine/PointOnEarthCalculator.kt b/libs/ballistics-lib/src/main/kotlin/ballistics/flightLine/PointOnEarthCalculator.kt index 9a78783..e66ff34 100644 --- a/libs/ballistics-lib/src/main/kotlin/ballistics/flightLine/PointOnEarthCalculator.kt +++ b/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, diff --git a/services/pcp-ballistics-service/src/main/kotlin/space/nstart/pcp/pcp_request_service/angular/AngularMotionService.kt b/services/pcp-ballistics-service/src/main/kotlin/space/nstart/pcp/pcp_request_service/angular/AngularMotionService.kt index 7f63265..874c086 100644 --- a/services/pcp-ballistics-service/src/main/kotlin/space/nstart/pcp/pcp_request_service/angular/AngularMotionService.kt +++ b/services/pcp-ballistics-service/src/main/kotlin/space/nstart/pcp/pcp_request_service/angular/AngularMotionService.kt @@ -21,6 +21,7 @@ import space.nstart.pcp.pcp_types_lib.dto.ballistics.ExactTimePositionRequestDTO import space.nstart.pcp.pcp_types_lib.dto.ballistics.OrbPointDTO import kotlin.math.PI import kotlin.math.abs +import kotlin.math.atan import kotlin.math.max import kotlin.math.roundToLong @@ -91,7 +92,8 @@ class AngularMotionService( } catch (error: RuntimeException) { throw CustomValidationException(error.message ?: "Ошибка расчета ПУУД") } - val contourWkt = SurveyContourCalculator(EarthType.PZ90d02).calculate(result, request.captureAngleDeg) + val contourWkt = SurveyContourCalculator(EarthType.PZ90d02).calculate(result, 0.0, AngularMotionConfig().focus * atan(request.captureAngleDeg * PI / 180), + AngularMotionConfig().focus) return AngularMotionCalculationResultDTO( mode = result.mode,