From 69f2d6ad4f6b117d9d10cafe2f7a52f06d66a909 Mon Sep 17 00:00:00 2001 From: emelianov Date: Mon, 8 Jun 2026 18:43:05 +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=D0=BE=D0=B9=20=D0=BE?= =?UTF-8?q?=D1=80=D0=B8=D0=B5=D0=BD=D1=82=D0=B0=D1=86=D0=B8=D0=B5=D0=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../angularmotion/AbstractPuudCalculator.kt | 102 ++++++++++++++---- .../pcp/angularmotion/ConstOrientPUUD.kt | 60 ++++++++--- .../nstart/pcp/angularmotion/InternalMath.kt | 8 +- .../AngularMotionCalculatorSmokeTest.kt | 2 +- .../ballistics/mpl/OrientOnPointCalculator.kt | 2 +- 5 files changed, 135 insertions(+), 39 deletions(-) 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 789b82e..f790ec3 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 @@ -137,7 +137,7 @@ abstract class AbstractPuudCalculator( b: Double, l: Double, h: Double, - routeDirection: Vector3D? = null, + routeDirectionGsk: Vector3D? = null, ): Orientation { val ka = pointAt(t) val targetGsk = astro.earth.blh2xyz(b, l, h) @@ -145,13 +145,40 @@ abstract class AbstractPuudCalculator( val compensated = rotationZ(-lineAngle(nlv)) * w val orient = orientationFromOrbitalVector(compensated) - if (routeDirection != null && routeDirection.module() > 1.0e-8) { - val route = rotationZ(-lineAngle(nlv)) * routeDirection - orient.risk = normalizeAngle(atan2(route.x, route.z)) + if (routeDirectionGsk != null && routeDirectionGsk.module() > 1.0e-8) { + orient.risk = riskFromGroundDirection(ka, nlv, orient, routeDirectionGsk) } return orient } + /** + * Подбор рысканья по физическому смыслу условия Wz/D = 0. + * + * Крен и тангаж уже наводят ЦЛВ на расчетную точку. Оставшаяся степень свободы — + * поворот вокруг ЦЛВ. Переводим направление движения точки визирования с Земли в + * ПСК при нулевом рысканье и поворачиваем ПСК так, чтобы поперечная компонента по + * OZ стала равной нулю, то есть чтобы движение изображения шло вдоль OX. + */ + private fun riskFromGroundDirection( + kaGsk: OrbitalPoint, + nlv: Int, + orientWithoutRisk: Orientation, + routeDirectionGsk: Vector3D, + ): Double { + val kaAbs = astro.grinvToASK(kaGsk) + 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 + return if (sqrt(wx * wx + wz * wz) < 1.0e-10) { + 0.0 + } else { + normalizeAngle(atan2(-wz, wx)) + } + } + protected fun pointInOrbitalFrame(kaGsk: OrbitalPoint, pointGsk: Vector3D): Vector3D { val kaAbs = astro.grinvToASK(kaGsk) val pointAbs = astro.grinvToASK(pointGsk, kaGsk.t) @@ -206,26 +233,59 @@ abstract class AbstractPuudCalculator( /** * Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для заданной линии визирования. - * Расчет оставлен самодостаточным и опирается на AstronomerJ2000/Vector3D/Matrix3D из ballistics-lib. + * + * W — относительная скорость движения по земной поверхности точки пересечения ЛВ с Землей. + * Это не скорость КА: из скорости спутника вычитается переносное вращение Земли, а затем + * добавляется радиальная составляющая изменения наклонной дальности, чтобы точка оставалась + * на поверхности эллипсоида. Такая схема соответствует формулам из OrbitalMotion::AbstractAISTPUUD::wd. */ protected fun wd(point: OrbitalPoint, nlv: Int, orientation: Orientation, omegaConnected: Vector3D): Vector3D { val ask = astro.grinvToASK(point) - val line = lineOfSightVectorInConnected(lineAngle(nlv)) - val bodyToOrbit = orientationMatrix(orientation) - val lineOrbit = bodyToOrbit * line - val lineAbs = (absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose() * lineOrbit).normSafe() - val ground = earthIntersection(point.r, (Matrix3D().also { it.makeOzMatrix(-astro.si2000(point.t)) } * lineAbs).normSafe()) - ?: return Vector3D() - val groundGsk = astro.earth.blh2xyz(ground) - val groundAbs = astro.grinvToASK(groundGsk, point.t) - val slantAbs = groundAbs - ask.r - val range = slantAbs.module().coerceAtLeast(EPS) - val omegaEarth = Vector3D(0.0, 0.0, astro.earth.wEarth) - val relVelocityAbs = ask.v - omegaEarth.rem(ask.r) - val connectedToAbs = absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose() * orientationMatrix(orientation) - val relVelocityConnected = connectedToAbs.transpose() * relVelocityAbs - val compensation = omegaConnected.rem(line) * range - return (relVelocityConnected - compensation) / range + val lineConnected = lineOfSightVectorInConnected(lineAngle(nlv)) + val connectedToOrbit = orientationMatrix(orientation) + val absToOrbBook = absToOrbBookMatrix(ask.r, ask.v) + val orbitBookToAbs = absToOrbBook.transpose() + val orbitToOrbitBook = orbBookToOrbMatrix().transpose() + val absToGsk = Matrix3D().also { it.makeOzMatrix(-astro.si2000(point.t)) } + + val lineGsk = (absToGsk * orbitBookToAbs * orbitToOrbitBook * connectedToOrbit * lineConnected).normSafe() + if (lineGsk.module() < EPS) return Vector3D() + + val earth = astro.earth + val kalpha = 1.0 - earth.alphaEllips * earth.ekvRadius / earth.ekvRadius + val reducedR = Vector3D(point.r.x, point.r.y, point.r.z / kalpha) + val reducedLine = Vector3D(lineGsk.x, lineGsk.y, lineGsk.z / kalpha) + val reducedProjection = reducedR * reducedLine + val reducedLine2 = (reducedLine * reducedLine).coerceAtLeast(EPS) + val underRoot = reducedProjection * reducedProjection + + reducedLine2 * (earth.ekvRadius * earth.ekvRadius - reducedR * reducedR) + if (underRoot < 0.0 || !underRoot.isFinite()) return Vector3D() + + val range = -(reducedProjection + sqrt(underRoot)) / reducedLine2 + if (!range.isFinite() || abs(range) < EPS) return Vector3D() + + val slantGsk = lineGsk * range + val groundGsk = point.r + slantGsk + val reducedGround = Vector3D(groundGsk.x, groundGsk.y, groundGsk.z / (kalpha * kalpha)) + + var omegaGsk = connectedToOrbit * omegaConnected + omegaGsk = orbitToOrbitBook * omegaGsk + omegaGsk = orbitBookToAbs * omegaGsk + omegaGsk = absToGsk * omegaGsk + + val omegaEarth = Vector3D(0.0, 0.0, earth.wEarth) + val omegaRelativeToEarth = omegaGsk - omegaEarth + val relativeVelocity = point.v + omegaRelativeToEarth.rem(slantGsk) + val rangeRateCompensation = lineGsk * (-(reducedGround * relativeVelocity) / (reducedGround * lineGsk).coerceAwayFromZero()) + var wGround = relativeVelocity + rangeRateCompensation + + wGround = Matrix3D().also { it.makeOzMatrix(astro.si2000(point.t)) } * wGround + 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 { 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 e5432fd..28bce90 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 @@ -1,8 +1,10 @@ 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.Vector3D /** * Режим постоянной ориентации, порт ConstOrientAISTPUUD без AIST-префикса. @@ -36,29 +38,61 @@ class ConstOrientPUUD( var t = tn var elapsed = 0.0 var previous: AngularMotionPoint? = null - var b = id.b - var l = id.l - var h = id.h + + // В режиме ConstOrient ЦЛВ не должна сопровождать одну и ту же земную точку. + // На старте фиксируем направление ЦЛВ в ОСК, а далее на каждом шаге заново + // пересекаем эту фиксированную ЦЛВ с Землей. Важно, чтобы первая точка маршрута + // тоже была получена через такое пересечение: иначе первый шаг сшивает заданную + // исходную ЦЛМ с расчетной трассой постоянной ЦЛВ и дает искусственный скачок рысканья. + val fixedClvOrientation = orientOnPoint(id.nlv, tn, id.b, id.l, id.h) while (elapsed <= duration + 2.0 * step + EPS) { - val routeDirection = routeDirectionInOrbitalFrame(id.copy(b = b, l = l, h = h), t) - val orientation = orientOnPoint(id.nlv, t, b, l, h, routeDirection) + val currentGround = pointOnFixedClv(t, id.nlv, fixedClvOrientation) + ?: BLHPoint(id.b, id.l, id.h) + val routeDirectionGsk = fixedClvGroundDirection(t, step, id.nlv, fixedClvOrientation, currentGround) + val orientation = orientOnPoint( + id.nlv, + t, + currentGround.lat, + currentGround.long, + currentGround.h, + routeDirectionGsk, + ) val point = buildPoint(t, id.nlv, orientation, previous) result += point previous = point - // В исходном ConstOrientAISTPUUD точка маршрута пересчитывается по новой - // точке пересечения ЛВ с Землей, что удерживает ЦЛВ в орбитальной СК. - point.groundPoint?.let { - b = it.lat - l = it.long - h = id.h - } - elapsed += step t += step } 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 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) + + return when { + previousGround != null && nextGround != null -> groundDirection(previousGround, nextGround) + nextGround != null -> groundDirection(currentGround, nextGround) + previousGround != null -> groundDirection(previousGround, currentGround) + else -> Vector3D() + } + } + + private fun groundDirection(from: BLHPoint, to: BLHPoint): Vector3D { + val first = astro.earth.blh2xyz(from.lat, from.long, from.h) + val second = astro.earth.blh2xyz(to.lat, to.long, to.h) + return second - first + } } 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 17a275f..a464d0a 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 @@ -144,9 +144,11 @@ internal fun conToOpticMatrix(): Matrix3D = Matrix3D( ) internal fun lineOfSightVectorInConnected(lineAngle: Double): Vector3D { - // Центральная ЛВ в существующей ballistics-lib соответствует (0, 1, 0). - // Отклонение ЛВ переносится как поворот вокруг OZ связанной СК. - return Vector3D(-sin(lineAngle), cos(lineAngle), 0.0).normSafe() + // Центральная линия визирования направлена из центра КА к Земле. + // В программной/связанной СК при нулевых углах +OY направлена от Земли, + // поэтому ЦЛВ соответствует вектору (0, -1, 0). Отклонения боковых ЛВ + // задаются поворотом вокруг OZ связанной СК, как в OrbitalMotion::conToCLV. + return Vector3D(sin(lineAngle), -cos(lineAngle), 0.0).normSafe() } internal fun quaternionFromMatrix(m: Matrix3D): Quaternion3D = Quaternion3D().also { it.fromMatrixStanley(m) }.normalized() 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 e660b93..22c6211 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 @@ -116,7 +116,7 @@ class AngularMotionCalculatorSmokeTest { assert(stepper != null) if (stepper == null) return - val calculator = AzimuthPUUD(stepper, EarthType.PZ90d02) + val calculator = ConstOrientPUUD(stepper, EarthType.PZ90d02) val id = SurveyId( oep = listOf(false, false, false, true), nlv = 5, diff --git a/libs/ballistics-lib/src/main/kotlin/ballistics/mpl/OrientOnPointCalculator.kt b/libs/ballistics-lib/src/main/kotlin/ballistics/mpl/OrientOnPointCalculator.kt index 12e2cb4..1f2f272 100644 --- a/libs/ballistics-lib/src/main/kotlin/ballistics/mpl/OrientOnPointCalculator.kt +++ b/libs/ballistics-lib/src/main/kotlin/ballistics/mpl/OrientOnPointCalculator.kt @@ -17,7 +17,7 @@ import kotlin.math.sign import kotlin.math.sin import kotlin.math.sqrt -internal class OrientOnPointCalculator(val earthType: EarthType, val wcs: WorkCSType, val tangType: TangageType) { +class OrientOnPointCalculator(val earthType: EarthType, val wcs: WorkCSType, val tangType: TangageType) { var astro = AstronomerJ2000(earthType) fun pointInWCS(