From faf88eaf940da9e6455e8a8991f458bec2085f3b Mon Sep 17 00:00:00 2001 From: emelianov Date: Tue, 9 Jun 2026 12:31:53 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9F=D0=A3=D0=A3=D0=94=20=D1=81=20=D0=BF?= =?UTF-8?q?=D0=BE=D1=81=D1=82=D0=BE=D1=8F=D0=BD=D0=BD=D1=8B=D0=BC=20=D0=B0?= =?UTF-8?q?=D0=B7=D0=B8=D0=BC=D1=83=D1=82=D0=BE=D0=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libs/angular-motion-lib/README.md | 3 +- .../angularmotion/AbstractPuudCalculator.kt | 99 +++++++--------- .../pcp/angularmotion/AngularMotionTypes.kt | 19 +-- .../nstart/pcp/angularmotion/AzimuthPUUD.kt | 108 ++++++++---------- .../pcp/angularmotion/ConstOrientPUUD.kt | 22 ++-- .../nstart/pcp/angularmotion/InternalMath.kt | 9 +- .../nstart/pcp/angularmotion/SmoothSDIPUUD.kt | 2 +- .../AngularMotionCalculatorSmokeTest.kt | 79 ++++++++++++- 8 files changed, 184 insertions(+), 157 deletions(-) diff --git a/libs/angular-motion-lib/README.md b/libs/angular-motion-lib/README.md index 7da18ea..b9f7f99 100644 --- a/libs/angular-motion-lib/README.md +++ b/libs/angular-motion-lib/README.md @@ -15,6 +15,8 @@ - `ballistics.utils.astro.AstronomerJ2000`; - `ballistics.utils.math.Vector3D`, `Matrix3D`, `Quaternion3D`. +В текущей модели спутника используется одна центральная линия визирования. Номер линии визирования и отклонения боковых линий из расчетного API удалены. + Основной API: ```kotlin @@ -25,7 +27,6 @@ val calculator = AngularMotionCalculatorFactory.create( val result = calculator.calculate( SurveyId( - nlv = 4, t = startTimeSeconds, b = latitudeRad, l = longitudeRad, diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt index f790ec3..05d5d73 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt +++ b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt @@ -25,7 +25,6 @@ abstract class AbstractPuudCalculator( protected val astro = AstronomerJ2000(earthType) protected fun validate(id: SurveyId) { - require(id.nlv in 1..7) { "Номер линии визирования должен быть в диапазоне 1..7" } require(id.duration >= 0.0) { "Длительность режима не может быть отрицательной" } require(config.focus > 0.0) { "Фокусное расстояние должно быть положительным" } } @@ -36,27 +35,6 @@ abstract class AbstractPuudCalculator( protected fun pointAt(t: Double): OrbitalPoint = stepper.calculate(t) ?: throw AngularMotionCalculationException("Ошибка выхода на заданное время: $t") - protected fun lineAngle(nlv: Int): Double = - when (nlv) { - 1 -> config.psk1Angle + config.lv1Angle - 2 -> config.psk1Angle + config.lv2Angle - 3 -> config.psk1Angle + config.lv3Angle - 4 -> config.psk3Angle + config.lv4Angle - 5 -> config.psk2Angle + config.lv5Angle - 6 -> config.psk2Angle + config.lv6Angle - 7 -> config.psk2Angle + config.lv7Angle - else -> 0.0 - } - - protected fun oepNlv(oep: Int): Int = - when (oep) { - 1 -> 3 - 2 -> 1 - 3 -> 7 - 4 -> 5 - else -> 4 - } - protected fun sdiAt(sdi: List, elapsed: Double): Double = if (sdi.isEmpty()) { -1.0 @@ -129,10 +107,9 @@ abstract class AbstractPuudCalculator( } private fun equationValue(id: SurveyId, t: Double): Double = - orientOnPoint(id.nlv, t, id.b, id.l, id.h).tang - id.uprAngle + orientOnPoint(t, id.b, id.l, id.h).tang - id.uprAngle protected fun orientOnPoint( - nlv: Int, t: Double, b: Double, l: Double, @@ -141,12 +118,11 @@ abstract class AbstractPuudCalculator( ): Orientation { val ka = pointAt(t) val targetGsk = astro.earth.blh2xyz(b, l, h) - val w = pointInOrbitalFrame(ka, targetGsk) - val compensated = rotationZ(-lineAngle(nlv)) * w - val orient = orientationFromOrbitalVector(compensated) + val targetInOrbit = pointInOrbitalFrame(ka, targetGsk) + val orient = orientationFromOrbitalVector(targetInOrbit) if (routeDirectionGsk != null && routeDirectionGsk.module() > 1.0e-8) { - orient.risk = riskFromGroundDirection(ka, nlv, orient, routeDirectionGsk) + orient.risk = riskFromGroundDirection(ka, orient, routeDirectionGsk) } return orient } @@ -161,7 +137,6 @@ abstract class AbstractPuudCalculator( */ private fun riskFromGroundDirection( kaGsk: OrbitalPoint, - nlv: Int, orientWithoutRisk: Orientation, routeDirectionGsk: Vector3D, ): Double { @@ -169,9 +144,8 @@ abstract class AbstractPuudCalculator( val routeAbs = astro.grinvToASK(routeDirectionGsk, kaGsk.t) val routeOrbit = orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v) * routeAbs val routeConnected = orientationMatrix(orientWithoutRisk).transpose() * routeOrbit - val routeClv = rotationZ(-lineAngle(nlv)) * routeConnected - val wx = routeClv.x - val wz = routeClv.z + val wx = routeConnected.x + val wz = routeConnected.z return if (sqrt(wx * wx + wz * wz) < 1.0e-10) { 0.0 } else { @@ -192,14 +166,13 @@ abstract class AbstractPuudCalculator( return Orientation(tang, kren, 0.0) } - protected fun pointOnEarth(point: OrbitalPoint, nlv: Int, orientation: Orientation): BLHPoint? { + protected fun pointOnEarth(point: OrbitalPoint, orientation: Orientation): BLHPoint? { val ask = astro.grinvToASK(point) val orbitToAbs = absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose() val g = Matrix3D().also { it.makeOzMatrix(astro.si2000(point.t)) }.transpose() val orbitToGsk = g * orbitToAbs val connectedToOrbit = orientationMatrix(orientation) - val dConnected = lineOfSightVectorInConnected(lineAngle(nlv)) - val direction = (orbitToGsk * connectedToOrbit * dConnected).normSafe() + val direction = (orbitToGsk * connectedToOrbit * lineOfSightVectorInConnected()).normSafe() return earthIntersection(point.r, direction) } @@ -225,23 +198,20 @@ abstract class AbstractPuudCalculator( return quaternionFromMatrix(absToConnected.transpose()).normalized() } - protected fun wdForLines(point: OrbitalPoint, orientation: Orientation, omega: Vector3D): List = - (1..7).map { nlv -> wd(point, nlv, orientation, omega) } - - protected fun sdiForWd(wd: List, sickle: Boolean = false): List = - wd.map { (if (sickle) it.z else it.x) * config.focus } + protected fun sdiForWd(wd: Vector3D, sickle: Boolean = false): Double = + (if (sickle) wd.z else wd.x) * config.focus /** - * Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для заданной линии визирования. + * Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для единственной центральной линии визирования. * - * W — относительная скорость движения по земной поверхности точки пересечения ЛВ с Землей. + * W — относительная скорость движения по земной поверхности точки пересечения ЦЛВ с Землей. * Это не скорость КА: из скорости спутника вычитается переносное вращение Земли, а затем * добавляется радиальная составляющая изменения наклонной дальности, чтобы точка оставалась * на поверхности эллипсоида. Такая схема соответствует формулам из OrbitalMotion::AbstractAISTPUUD::wd. */ - protected fun wd(point: OrbitalPoint, nlv: Int, orientation: Orientation, omegaConnected: Vector3D): Vector3D { + protected fun wd(point: OrbitalPoint, orientation: Orientation, omegaConnected: Vector3D): Vector3D { val ask = astro.grinvToASK(point) - val lineConnected = lineOfSightVectorInConnected(lineAngle(nlv)) + val lineConnected = lineOfSightVectorInConnected() val connectedToOrbit = orientationMatrix(orientation) val absToOrbBook = absToOrbBookMatrix(ask.r, ask.v) val orbitBookToAbs = absToOrbBook.transpose() @@ -283,25 +253,41 @@ abstract class AbstractPuudCalculator( wGround = absToOrbBook * wGround wGround = orbBookToOrbMatrix() * wGround wGround = connectedToOrbit.transpose() * wGround - wGround = rotationZ(-lineAngle(nlv)) * wGround return wGround / range } protected fun routeNormalInGreenwich(b: Double, l: Double, h: Double, azimuth: Double): Vector3D { - val first = astro.earth.blh2xyz(b, l, h) - val secondBlh = endPointByAzimuth(b, l, azimuth, 0.1 * PI / 180.0) - val second = astro.earth.blh2xyz(secondBlh.lat, secondBlh.long, h) - return first.rem(second).normSafe() + // Центральная линия маршрута по ТЗ задается сечением ОЗЭ плоскостью, + // построенной в начальной точке по нормали к ОЗЭ и касательной к ОЗЭ + // с заданным азимутом. Нормаль такой плоскости — вектор normal x tangent. + val normal = ellipsoidNormalInGreenwich(b, l) + val tangent = routeDirectionInGreenwich(b, l, azimuth) + return normal.rem(tangent).normSafe() + } + + protected fun routeDirectionInGreenwich(b: Double, l: Double, azimuth: Double): Vector3D { + val north = northDirectionInGreenwich(b, l) + val east = eastDirectionInGreenwich(l) + return (north * cos(azimuth) + east * sin(azimuth)).normSafe() } protected fun routeDirectionInOrbitalFrame(id: SurveyId, t: Double): Vector3D { - val p1 = astro.earth.blh2xyz(id.b, id.l, id.h) - val p2 = astro.earth.blh2xyz(endPointByAzimuth(id.b, id.l, id.azimuth, 0.1 * PI / 180.0)) val ka = pointAt(t) - return pointInOrbitalFrame(ka, p2) - pointInOrbitalFrame(ka, p1) + val kaAbs = astro.grinvToASK(ka) + val routeAbs = astro.grinvToASK(routeDirectionInGreenwich(id.b, id.l, id.azimuth), t) + return orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v) * routeAbs } + private 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 = + Vector3D(-sin(l), cos(l), 0.0).normSafe() + + private fun northDirectionInGreenwich(b: Double, l: Double): Vector3D = + Vector3D(-sin(b) * cos(l), -sin(b) * sin(l), cos(b)).normSafe() + protected fun endPointByAzimuth(b: Double, l: Double, azimuth: Double, centralAngle: Double): BLHPoint { val sinB = sin(b) val cosB = cos(b) @@ -314,7 +300,6 @@ abstract class AbstractPuudCalculator( protected fun buildPoint( t: Double, - nlv: Int, orientation: Orientation, previous: AngularMotionPoint? = null, sickle: Boolean = false, @@ -323,8 +308,8 @@ abstract class AbstractPuudCalculator( val q = quaternionFor(orbital, orientation) val omega = previous?.let { omegaFromTwoQuat(it.quaternion, q, t - it.t) } ?: Vector3D() val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D() - val ground = pointOnEarth(orbital, nlv, orientation) - val wd = wdForLines(orbital, orientation, omega) + val ground = pointOnEarth(orbital, orientation) + val wd = wd(orbital, orientation, omega) return AngularMotionPoint( t = t, orbitalPoint = orbital, @@ -338,7 +323,7 @@ abstract class AbstractPuudCalculator( ) } - protected fun calculateTauPoints(points: List, nlv: Int): List { + protected fun calculateTauPoints(points: List): List { if (points.size < 3 || config.tau <= 0.0) return emptyList() val step = calculationStep() val baseTime = points.first().t @@ -358,7 +343,7 @@ abstract class AbstractPuudCalculator( lagrange(x, xs, kren), lagrange(x, xs, risk), ) - val point = buildPoint(t, nlv, orientation, previous) + val point = buildPoint(t, orientation, previous) result += point previous = point d -= step diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionTypes.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionTypes.kt index 53ea90f..eba5445 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionTypes.kt +++ b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionTypes.kt @@ -5,7 +5,6 @@ import ballistics.types.OrbitalPoint import ballistics.types.Orientation import ballistics.utils.math.Quaternion3D import ballistics.utils.math.Vector3D -import kotlin.math.PI /** * Параметры блока состояния КА, перенесенные из sBLS_KA проекта OrbitalMotion. @@ -22,18 +21,6 @@ data class AngularMotionConfig( val discreteTimes: Double = 0.125, /** Длительность интервала успокоения, с. */ val tau: Double = 10.0, - /** Углы отклонения программных СК. */ - val psk1Angle: Double = 19.5 * PI / 180.0, - val psk3Angle: Double = 0.0, - val psk2Angle: Double = -19.5 * PI / 180.0, - /** Углы отклонения линий визирования. */ - val lv1Angle: Double = 0.01875, - val lv2Angle: Double = 0.0, - val lv3Angle: Double = -0.01875, - val lv4Angle: Double = 0.0, - val lv5Angle: Double = 0.01875, - val lv6Angle: Double = 0.0, - val lv7Angle: Double = -0.01875, /** Расстояние от центра ОЭП до центра фокальной плоскости телескопа, мм. */ val dxOep: Double = 37.5, /** Длина линейки ОЭП, мм. */ @@ -53,8 +40,6 @@ enum class AngularMotionMode { data class SurveyId( /** Признаки включения ОЭП: 1..4. */ val oep: List = listOf(false, false, false, false), - /** Номер линии визирования: 1..7. */ - val nlv: Int = 4, /** Примерное интегральное время начала наблюдения, с. */ val t: Double = 0.0, /** Координаты точки прицеливания: широта/долгота в радианах, высота в метрах. */ @@ -89,8 +74,8 @@ data class AngularMotionPoint( val omega: Vector3D = Vector3D(), val eps: Vector3D = Vector3D(), val quaternion: Quaternion3D = Quaternion3D(1.0, 0.0, 0.0, 0.0), - val wd: List = List(7) { Vector3D() }, - val sdi: List = List(7) { 0.0 }, + val wd: Vector3D = Vector3D(), + val sdi: Double = 0.0, ) /** Полный результат расчета режима углового движения. */ diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt index 80f870c..ae941c5 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt +++ b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt @@ -1,7 +1,6 @@ package space.nstart.pcp.angularmotion import ballistics.orbitalPoints.timeStepper.AbstractStepper -import ballistics.types.BLHPoint import ballistics.types.EarthType import ballistics.types.Orientation import ballistics.utils.math.Matrix3D @@ -32,41 +31,35 @@ open class AzimuthPUUD( throw AngularMotionCalculationException("Некорректное значение СДИ") } - var tn = calcTn(id) - var workId = id - var duration = id.duration - var oeps = buildOeps(id, tn) - - // Логика специальных ЛВ 2/6 из AzimuthAISTPUUD: расширяем длительность на - // время прохода между линейками ОЭП и сдвигаем t_on/t_off парных ОЭП. - if (id.nlv == 2 || id.nlv == 6) { - val rvo = roundDownByDay(config.dxOep / id.sdi.first(), config.discreteTimes).coerceAtLeast(0.0) - duration += rvo * 2.0 - tn -= rvo - oeps = shiftedPairOeps(id, tn, rvo) - workId = id.copy(t = tn) - } - - val points = calculateAzimuth(workId, tn, duration) + val tn = calcTn(id) + val points = calculateAzimuth(id, tn, id.duration) return AngularMotionResult( mode = AngularMotionMode.AZIMUTH, startTime = tn, points = points, - tauPoints = calculateTauPoints(points, id.nlv), - oeps = oeps, + tauPoints = calculateTauPoints(points), + oeps = buildOeps(id, tn), ) } protected open fun calculateAzimuth(id: SurveyId, tn: Double, duration: Double): List { val step = calculationStep() val routeNormalGsk = routeNormalInGreenwich(id.b, id.l, id.h, id.azimuth) + val routeDirectionGsk = routeDirectionInGreenwich(id.b, id.l, id.azimuth) val firstTargetGsk = astro.earth.blh2xyz(id.b, id.l, id.h) - var currentSdi = sdiAt(id.sdi, 0.0) + val initialSdi = sdiAt(id.sdi, 0.0) val firstPoint = pointAt(tn) val firstAbs = astro.grinvToASK(firstPoint) val targetAbs = astro.grinvToASK(firstTargetGsk, tn) - val initialLiv = initialVisirQuaternion(firstAbs.r, targetAbs, routeNormalGsk, tn, currentSdi < 0.0) + val initialLiv = initialVisirQuaternion( + rAbs = firstAbs.r, + targetAbs = targetAbs, + routeNormalGsk = routeNormalGsk, + routeDirectionGsk = routeDirectionGsk, + time = tn, + reverse = initialSdi < 0.0, + ) var liv = initialLiv val result = mutableListOf() @@ -75,10 +68,9 @@ open class AzimuthPUUD( var previous: AngularMotionPoint? = null while (elapsed <= duration + EPS) { - currentSdi = sdiAt(id.sdi, elapsed) val orbital = pointAt(t) - val orientation = orientationFromVisirQuaternion(orbital, id.nlv, liv) - val point = buildIntegratedPoint(t, id.nlv, orientation, liv, previous, sickle) + val orientation = orientationFromVisirQuaternion(orbital, liv) + val point = buildIntegratedPoint(t, orientation, liv, previous, sickle) result += point previous = point @@ -87,7 +79,8 @@ open class AzimuthPUUD( val p = pointAt(time) val ask = astro.grinvToASK(p) val di = slantRangeFromQuaternion(q, ask.r) - ownCornerSpeed(time, q, di, ask.r, ask.v, currentSdi) + val sdiForTime = sdiAt(id.sdi, time - tn) + ownCornerSpeed(time, q, di, ask.r, ask.v, sdiForTime) } } elapsed += step @@ -120,7 +113,6 @@ open class AzimuthPUUD( protected fun buildIntegratedPoint( t: Double, - nlv: Int, orientation: Orientation, liv: Quaternion3D, previous: AngularMotionPoint?, @@ -129,8 +121,8 @@ open class AzimuthPUUD( val orbital = pointAt(t) val omega = previous?.let { omegaFromTwoQuat(it.quaternion, liv, t - it.t) } ?: Vector3D() val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D() - val ground = pointOnEarth(orbital, nlv, orientation) - val wd = wdForLines(orbital, orientation, omega) + val ground = pointOnEarth(orbital, orientation) + val wd = wd(orbital, orientation, omega) return AngularMotionPoint( t = t, orbitalPoint = orbital, @@ -148,25 +140,36 @@ open class AzimuthPUUD( rAbs: Vector3D, targetAbs: Vector3D, routeNormalGsk: Vector3D, + routeDirectionGsk: Vector3D, time: Double, reverse: Boolean, ): Quaternion3D { - var normalAbs = astro.grinvToASK(routeNormalGsk, time).normSafe() - if (reverse) normalAbs = normalAbs * -1.0 - val di = targetAbs - rAbs - val eY = normalAbs.rem(targetAbs.normSafe()).normSafe() - val eZ = eY.rem(di.normSafe()).normSafe() - var m = Matrix3D( - di.normSafe(), - eZ.rem(di.normSafe()).normSafe(), - eZ, - ) - if (sickle) { - m = rotationX(PI) * Matrix3D( - di.normSafe(), - eZ, - di.normSafe().rem(eZ).normSafe(), + val line = (targetAbs - rAbs).normSafe() + val routeNormalAbs = astro.grinvToASK(routeNormalGsk, time).normSafe() + val routeDirectionAbs = astro.grinvToASK(routeDirectionGsk, time).normSafe() + + // Ось движения изображения в визирной СК должна быть связана с касательной + // центральной линии маршрута, а не с хордой двух земных радиус-векторов. + // Проекция азимутальной касательной в фокальную плоскость соответствует + // критериям Wz/D = 0 и Wx/D = const: поперечная составляющая обнуляется, + // а продольная ось направлена по маршруту. + var routeAxis = (routeDirectionAbs - line * (routeDirectionAbs * line)).normSafe() + if (routeAxis.module() < EPS) { + routeAxis = routeNormalAbs.rem(line).normSafe() + } + if (reverse) { + routeAxis = routeAxis * -1.0 + } + + val crossAxis = line.rem(routeAxis).normSafe() + val m = if (sickle) { + rotationX(PI) * Matrix3D( + line, + crossAxis, + line.rem(crossAxis).normSafe(), ) + } else { + Matrix3D(line, routeAxis, crossAxis) } return quaternionFromMatrix(m).inverse().normalized() } @@ -182,29 +185,14 @@ open class AzimuthPUUD( return lineAbs * root } - protected fun orientationFromVisirQuaternion(orbital: ballistics.types.OrbitalPoint, nlv: Int, liv: Quaternion3D): Orientation { + protected fun orientationFromVisirQuaternion(orbital: ballistics.types.OrbitalPoint, liv: Quaternion3D): Orientation { val ask = astro.grinvToASK(orbital) val ob = orbBookToOrbMatrix() val absOb = absToOrbBookMatrix(ask.r, ask.v) val absToOrbit = ob * absOb - val oepToConnected = rotationZ(-lineAngle(nlv)).transpose() val lConToVisir = Quaternion3D(0.0, sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0) - val connectedToAbs = (liv * lConToVisir.inverse() * quaternionFromMatrix(oepToConnected).inverse()).matrix().transpose() + val connectedToAbs = (liv * lConToVisir.inverse()).matrix().transpose() val orbitToConnected = connectedToAbs * absToOrbit.transpose() return anglesFromOrbToCon(orbitToConnected) } - - private fun shiftedPairOeps(id: SurveyId, tn: Double, rvo: Double): List { - val base = MutableList(4) { index -> - if (id.oep.getOrNull(index) == true) OepResult(true, tn, tn + id.duration) else OepResult(false) - } - if (id.nlv == 2) { - base[0] = OepResult(id.oep.getOrNull(0) == true, tn + rvo * 2.0, tn + rvo * 2.0 + id.duration) - base[1] = OepResult(id.oep.getOrNull(1) == true, tn, tn + id.duration) - } else if (id.nlv == 6) { - base[2] = OepResult(id.oep.getOrNull(2) == true, tn + rvo * 2.0, tn + rvo * 2.0 + id.duration) - base[3] = OepResult(id.oep.getOrNull(3) == true, tn, tn + id.duration) - } - return base - } } diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt index 28bce90..7c9c4d2 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt +++ b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt @@ -11,7 +11,7 @@ import ballistics.utils.math.Vector3D * * Базовая логика повторяет исходный класс: уточняется время начала по упреждающему * углу, затем с дискретом stepPuud рассчитываются углы ориентации, точка пересечения - * ЛВ с Землей, кватернион, угловая скорость, W/D и СДИ по всем семи ЛВ. + * единственной ЦЛВ с Землей, кватернион, угловая скорость, W/D и СДИ. */ class ConstOrientPUUD( stepper: AbstractStepper, @@ -27,7 +27,7 @@ class ConstOrientPUUD( mode = AngularMotionMode.CONST_ORIENT, startTime = tn, points = points, - tauPoints = calculateTauPoints(points, id.nlv), + tauPoints = calculateTauPoints(points), oeps = buildOeps(id, tn), ) } @@ -44,21 +44,20 @@ class ConstOrientPUUD( // пересекаем эту фиксированную ЦЛВ с Землей. Важно, чтобы первая точка маршрута // тоже была получена через такое пересечение: иначе первый шаг сшивает заданную // исходную ЦЛМ с расчетной трассой постоянной ЦЛВ и дает искусственный скачок рысканья. - val fixedClvOrientation = orientOnPoint(id.nlv, tn, id.b, id.l, id.h) + val fixedClvOrientation = orientOnPoint(tn, id.b, id.l, id.h) while (elapsed <= duration + 2.0 * step + EPS) { - val currentGround = pointOnFixedClv(t, id.nlv, fixedClvOrientation) + val currentGround = pointOnFixedClv(t, fixedClvOrientation) ?: BLHPoint(id.b, id.l, id.h) - val routeDirectionGsk = fixedClvGroundDirection(t, step, id.nlv, fixedClvOrientation, currentGround) + val routeDirectionGsk = fixedClvGroundDirection(t, step, fixedClvOrientation, currentGround) val orientation = orientOnPoint( - id.nlv, t, currentGround.lat, currentGround.long, currentGround.h, routeDirectionGsk, ) - val point = buildPoint(t, id.nlv, orientation, previous) + val point = buildPoint(t, orientation, previous) result += point previous = point @@ -69,18 +68,17 @@ class ConstOrientPUUD( return if (result.size > 2) result.dropLast(2) else result } - private fun pointOnFixedClv(t: Double, nlv: Int, fixedClvOrientation: Orientation): BLHPoint? = - pointOnEarth(pointAt(t), nlv, fixedClvOrientation) + private fun pointOnFixedClv(t: Double, fixedClvOrientation: Orientation): BLHPoint? = + pointOnEarth(pointAt(t), fixedClvOrientation) private fun fixedClvGroundDirection( t: Double, step: Double, - nlv: Int, fixedClvOrientation: Orientation, currentGround: BLHPoint, ): Vector3D { - val previousGround = pointOnFixedClv(t - step, nlv, fixedClvOrientation) - val nextGround = pointOnFixedClv(t + step, nlv, fixedClvOrientation) + val previousGround = pointOnFixedClv(t - step, fixedClvOrientation) + val nextGround = pointOnFixedClv(t + step, fixedClvOrientation) return when { previousGround != null && nextGround != null -> groundDirection(previousGround, nextGround) diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt index a464d0a..9872143 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt +++ b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt @@ -143,12 +143,11 @@ internal fun conToOpticMatrix(): Matrix3D = Matrix3D( Vector3D(0.0, 0.0, -1.0), ) -internal fun lineOfSightVectorInConnected(lineAngle: Double): Vector3D { - // Центральная линия визирования направлена из центра КА к Земле. +internal fun lineOfSightVectorInConnected(): Vector3D { + // У спутника используется единственная центральная линия визирования. // В программной/связанной СК при нулевых углах +OY направлена от Земли, - // поэтому ЦЛВ соответствует вектору (0, -1, 0). Отклонения боковых ЛВ - // задаются поворотом вокруг OZ связанной СК, как в OrbitalMotion::conToCLV. - return Vector3D(sin(lineAngle), -cos(lineAngle), 0.0).normSafe() + // поэтому ЦЛВ, направленная из центра КА к Земле, задается вектором (0, -1, 0). + return Vector3D(0.0, -1.0, 0.0) } internal fun quaternionFromMatrix(m: Matrix3D): Quaternion3D = Quaternion3D().also { it.fromMatrixStanley(m) }.normalized() diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SmoothSDIPUUD.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SmoothSDIPUUD.kt index 405516a..305fcf7 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SmoothSDIPUUD.kt +++ b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SmoothSDIPUUD.kt @@ -29,7 +29,7 @@ class SmoothSDIPUUD( mode = AngularMotionMode.SMOOTH_SDI, startTime = tn, points = points, - tauPoints = calculateTauPoints(points, id.nlv), + tauPoints = calculateTauPoints(points), oeps = buildOeps(id, tn), ) } 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 22c6211..29955e8 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 @@ -10,6 +10,8 @@ import ballistics.utils.fromDateTime import ballistics.utils.math.Vector3D import ballistics.utils.toDateTime import java.time.LocalDateTime +import java.time.format.DateTimeFormatter +import java.util.Locale import kotlin.math.PI import kotlin.math.cos import kotlin.math.sin @@ -18,11 +20,12 @@ import kotlin.test.assertEquals import kotlin.test.assertTrue class AngularMotionCalculatorSmokeTest { + private val outputTimeFormatter = DateTimeFormatter.ofPattern("yyyy-MM-dd'T'HH:mm:ss.SSS") + @Test fun `const orientation returns points`() { val result = ConstOrientPUUD(CircularStepper()).calculate( SurveyId( - nlv = 4, t = 1000.0, b = 0.1, l = 0.2, @@ -44,7 +47,6 @@ class AngularMotionCalculatorSmokeTest { val result = calculator.calculate( SurveyId( - nlv = 4, t = 1000.0, b = 0.1, l = 0.2, @@ -83,7 +85,7 @@ class AngularMotionCalculatorSmokeTest { @Test - fun chek(){ + fun chekConstOrient(){ val ballistics = Ballistics() val t = LocalDateTime.of(2023, 4, 12, 17, 41, 18, 0) val ic = InitialConditions( @@ -119,7 +121,6 @@ class AngularMotionCalculatorSmokeTest { val calculator = ConstOrientPUUD(stepper, EarthType.PZ90d02) val id = SurveyId( oep = listOf(false, false, false, true), - nlv = 5, t = fromDateTime(LocalDateTime.of(2023, 4, 16, 3, 48, 22, 0)), b = 49.25824 * PI / 180, l = 153.65914 * PI / 180, @@ -140,4 +141,74 @@ class AngularMotionCalculatorSmokeTest { println("${toDateTime( v.t)} ${v.orientation.tang * 180 / PI} ${v.orientation.kren * 180 / PI } ${v.orientation.risk * 180 / PI}") } } + + + @Test + fun chekAzimuth(){ + val ballistics = Ballistics() + val t = LocalDateTime.of(2023, 4, 12, 17, 41, 18, 0) + val ic = InitialConditions( + OrbitalPoint( + fromDateTime(t), + 1, + Vector3D( + -3101926.630, + 6018678.8, + 0.0, + ), + Vector3D( + 1287.651, + 663.634, + 7612.951, + ), + ), + 0.005, + 160.0 + ) + val result = ballistics.calculateOrbPoints( + ic, + ic.point.t, + ic.point.t + 86400 * 10 + ) + assertEquals(result, BallisticsError.OK) + if (result != BallisticsError.OK) + return + val stepper = ballistics.getStepper() + assert(stepper != null) + if (stepper == null) + return + val calculator = AzimuthPUUD(stepper, EarthType.PZ90d02) + val id = SurveyId( + oep = listOf(false, false, false, true), + t = fromDateTime(LocalDateTime.of(2023, 4, 16, 3, 48, 22, 0)), + b = 49.25824 * PI / 180, + l = 153.65914 * PI / 180, + h = 0.0, + duration = 69.0, + sdi = listOf(30.0), + azimuth = 25.567 * PI / 180, + uprAngle = 6.0 * PI / 180, + pointInCenter = false + ) + val rc = calculator.calculate(id) + + println(rc.mode) + println(rc.points.size) + println(rc.startTime) + + for (v in rc.points) { + println( + String.format( + Locale.US, + "%s %.3f %.3f %.3f , %s %s", + toDateTime(v.t).format(outputTimeFormatter), + v.orientation.tang * 180 / PI, + v.orientation.kren * 180 / PI, + v.orientation.risk * 180 / PI, + v.groundPoint?.let { String.format(Locale.US, "%.3f", it.lat * 180 / PI) }, + v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) }, + ) + ) + } + } }