расчет ПУУД

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

@@ -0,0 +1,308 @@
+package space.nstart.pcp.angularmotion
+
+import ballistics.orbitalPoints.timeStepper.AbstractStepper
+import ballistics.types.BLHPoint
+import ballistics.types.EarthType
+import ballistics.types.OrbitalPoint
+import ballistics.types.Orientation
+import ballistics.utils.astro.AstronomerJ2000
+import ballistics.utils.math.Matrix3D
+import ballistics.utils.math.Quaternion3D
+import ballistics.utils.math.Vector3D
+import kotlin.math.PI
+import kotlin.math.abs
+import kotlin.math.atan2
+import kotlin.math.cos
+import kotlin.math.floor
+import kotlin.math.sin
+import kotlin.math.sqrt
+
+abstract class AbstractPuudCalculator(
+    protected val stepper: AbstractStepper,
+    protected val earthType: EarthType = EarthType.PZ90d02,
+    protected val config: AngularMotionConfig = AngularMotionConfig(),
+) : AngularMotionCalculator {
+    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) { "Фокусное расстояние должно быть положительным" }
+    }
+
+    protected fun calculationStep(): Double =
+        if (config.stepPuud in 0.1..5.0) config.stepPuud else 0.125
+
+    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<Double>, elapsed: Double): Double =
+        if (sdi.isEmpty()) {
+            -1.0
+        } else {
+            val index = floor(elapsed / config.stepSdi).toInt()
+            sdi.getOrElse(index) { sdi.last() }
+        }
+
+    protected fun buildOeps(id: SurveyId, startTime: Double, duration: Double = id.duration): List<OepResult> =
+        (0 until 4).map { index ->
+            val state = id.oep.getOrNull(index) ?: false
+            if (state) OepResult(true, startTime, startTime + duration) else OepResult(false)
+        }
+
+    /**
+     * Поиск времени начала режима по упреждающему углу.
+     * В OrbitalMotion используется EquationCalculatorSpan на [t-180; t+180]. Здесь сохранена та же идея:
+     * сканирование интервала, уточнение корня бисекцией и округление вниз к дискрету времени начала.
+     */
+    protected fun calcTn(id: SurveyId): Double {
+        val start = id.t - 180.0
+        val end = id.t + 180.0
+        val scanStep = 5.0
+        var bestT = id.t
+        var bestAbs = Double.POSITIVE_INFINITY
+        var left = start
+        var leftValue = equationValue(id, left)
+        var bracket: Pair<Double, Double>? = null
+        var t = start + scanStep
+
+        while (t <= end + EPS) {
+            val value = equationValue(id, t)
+            val av = abs(value)
+            if (av < bestAbs) {
+                bestAbs = av
+                bestT = t
+            }
+            if (leftValue == 0.0 || value == 0.0 || kotlin.math.sign(leftValue) != kotlin.math.sign(value)) {
+                bracket = left to t
+                break
+            }
+            left = t
+            leftValue = value
+            t += scanStep
+        }
+
+        val raw = bracket?.let { (a0, b0) ->
+            var a = a0
+            var b = b0
+            var fa = equationValue(id, a)
+            repeat(60) {
+                val mid = (a + b) / 2.0
+                val fm = equationValue(id, mid)
+                if (abs(fm) < 1.0e-10) {
+                    a = mid
+                    b = mid
+                    return@repeat
+                }
+                if (kotlin.math.sign(fa) == kotlin.math.sign(fm)) {
+                    a = mid
+                    fa = fm
+                } else {
+                    b = mid
+                }
+            }
+            (a + b) / 2.0
+        } ?: bestT
+
+        return roundDownByDay(raw, config.discreteTimes)
+    }
+
+    private fun equationValue(id: SurveyId, t: Double): Double =
+        orientOnPoint(id.nlv, t, id.b, id.l, id.h).tang - id.uprAngle
+
+    protected fun orientOnPoint(
+        nlv: Int,
+        t: Double,
+        b: Double,
+        l: Double,
+        h: Double,
+        routeDirection: Vector3D? = null,
+    ): 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)
+
+        if (routeDirection != null && routeDirection.module() > 1.0e-8) {
+            val route = rotationZ(-lineAngle(nlv)) * routeDirection
+            orient.risk = normalizeAngle(atan2(route.x, route.z))
+        }
+        return orient
+    }
+
+    protected fun pointInOrbitalFrame(kaGsk: OrbitalPoint, pointGsk: Vector3D): Vector3D {
+        val kaAbs = astro.grinvToASK(kaGsk)
+        val pointAbs = astro.grinvToASK(pointGsk, kaGsk.t)
+        val m = orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v)
+        return m * (pointAbs - kaAbs.r)
+    }
+
+    protected fun orientationFromOrbitalVector(vector: Vector3D): Orientation {
+        val tang = normalizeAngle(PI - atan2(vector.x, vector.y))
+        val kren = normalizeAngle(atan2(vector.z, (vector.y / cos(tang).coerceAwayFromZero())) - PI)
+        return Orientation(tang, kren, 0.0)
+    }
+
+    protected fun pointOnEarth(point: OrbitalPoint, nlv: Int, 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()
+        return earthIntersection(point.r, direction)
+    }
+
+    protected fun earthIntersection(rotn: Vector3D, direction: Vector3D): BLHPoint? {
+        val d = direction.normSafe()
+        if ((rotn + d).module() > rotn.module()) return null
+
+        val a = astro.earth.ekvRadius
+        val b = astro.earth.polarRadius
+        val aa = b * b * d.x * d.x + b * b * d.y * d.y + a * a * d.z * d.z
+        val bb = 2.0 * b * b * d.x * rotn.x + 2.0 * b * b * d.y * rotn.y + 2.0 * a * a * d.z * rotn.z
+        val cc = b * b * rotn.x * rotn.x + b * b * rotn.y * rotn.y + a * a * rotn.z * rotn.z - a * a * b * b
+        val root = minPositiveRoot(aa, bb, cc) ?: return null
+        val current = rotn + d * root
+        return astro.earth.xyz2blh(current)
+    }
+
+    protected fun quaternionFor(point: OrbitalPoint, orientation: Orientation): Quaternion3D {
+        val ask = astro.grinvToASK(point)
+        val absToOrbit = orbBookToOrbMatrix() * absToOrbBookMatrix(ask.r, ask.v)
+        val orbitToConnected = orientationMatrix(orientation).transpose()
+        val absToConnected = absToOrbit.transpose() * orbitToConnected
+        return quaternionFromMatrix(absToConnected.transpose()).normalized()
+    }
+
+    protected fun wdForLines(point: OrbitalPoint, orientation: Orientation, omega: Vector3D): List<Vector3D> =
+        (1..7).map { nlv -> wd(point, nlv, orientation, omega) }
+
+    protected fun sdiForWd(wd: List<Vector3D>, sickle: Boolean = false): List<Double> =
+        wd.map { (if (sickle) it.z else it.x) * config.focus }
+
+    /**
+     * Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для заданной линии визирования.
+     * Расчет оставлен самодостаточным и опирается на AstronomerJ2000/Vector3D/Matrix3D из ballistics-lib.
+     */
+    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
+    }
+
+    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()
+    }
+
+    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)
+    }
+
+    protected fun endPointByAzimuth(b: Double, l: Double, azimuth: Double, centralAngle: Double): BLHPoint {
+        val sinB = sin(b)
+        val cosB = cos(b)
+        val sinD = sin(centralAngle)
+        val cosD = cos(centralAngle)
+        val lat = kotlin.math.asin(sinB * cosD + cosB * sinD * cos(azimuth))
+        val lon = l + atan2(sin(azimuth) * sinD * cosB, cosD - sinB * sin(lat))
+        return BLHPoint(lat, normalizeAngle(lon), 0.0)
+    }
+
+    protected fun buildPoint(
+        t: Double,
+        nlv: Int,
+        orientation: Orientation,
+        previous: AngularMotionPoint? = null,
+        sickle: Boolean = false,
+    ): AngularMotionPoint {
+        val orbital = pointAt(t)
+        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)
+        return AngularMotionPoint(
+            t = t,
+            orbitalPoint = orbital,
+            orientation = Orientation(orientation.tang, orientation.kren, orientation.risk),
+            groundPoint = ground,
+            omega = omega,
+            eps = eps,
+            quaternion = q,
+            wd = wd,
+            sdi = sdiForWd(wd, sickle),
+        )
+    }
+
+    protected fun calculateTauPoints(points: List<AngularMotionPoint>, nlv: Int): List<AngularMotionPoint> {
+        if (points.size < 3 || config.tau <= 0.0) return emptyList()
+        val step = calculationStep()
+        val baseTime = points.first().t
+        val source = points.take(minOf(6, points.size))
+        val xs = source.map { it.t - baseTime }
+        val tang = source.map { it.orientation.tang }
+        val kren = source.map { it.orientation.kren }
+        val risk = source.map { it.orientation.risk }
+        val result = mutableListOf<AngularMotionPoint>()
+        var d = config.tau
+        var previous: AngularMotionPoint? = null
+        while (d >= step - EPS) {
+            val t = baseTime - d
+            val x = t - baseTime
+            val orientation = Orientation(
+                lagrange(x, xs, tang),
+                lagrange(x, xs, kren),
+                lagrange(x, xs, risk),
+            )
+            val point = buildPoint(t, nlv, orientation, previous)
+            result += point
+            previous = point
+            d -= step
+        }
+        return result.sortedBy { it.t }
+    }
+}

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

@@ -0,0 +1,6 @@
+package space.nstart.pcp.angularmotion
+
+/** Расчетчик одного режима реализации углового движения. */
+interface AngularMotionCalculator {
+    fun calculate(id: SurveyId): AngularMotionResult
+}

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

@@ -0,0 +1,18 @@
+package space.nstart.pcp.angularmotion
+
+import ballistics.orbitalPoints.timeStepper.AbstractStepper
+import ballistics.types.EarthType
+
+object AngularMotionCalculatorFactory {
+    fun create(
+        mode: AngularMotionMode,
+        stepper: AbstractStepper,
+        earthType: EarthType = EarthType.PZ90d02,
+        config: AngularMotionConfig = AngularMotionConfig(),
+    ): AngularMotionCalculator =
+        when (mode) {
+            AngularMotionMode.CONST_ORIENT -> ConstOrientPUUD(stepper, earthType, config)
+            AngularMotionMode.AZIMUTH -> AzimuthPUUD(stepper, earthType, config)
+            AngularMotionMode.SMOOTH_SDI -> SmoothSDIPUUD(stepper, earthType, config)
+        }
+}

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

@@ -0,0 +1,105 @@
+package space.nstart.pcp.angularmotion
+
+import ballistics.types.BLHPoint
+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.
+ * Угловые величины задаются в радианах, линейные параметры оптики — в миллиметрах.
+ */
+data class AngularMotionConfig(
+    /** Фокусное расстояние, мм. */
+    val focus: Double = 2000.0,
+    /** Шаг записи СДИ, с. */
+    val stepSdi: Double = 20.0,
+    /** Дискретность расчета ПУУД, с. */
+    val stepPuud: Double = 0.125,
+    /** Дискретность времени начала режима, с. */
+    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,
+    /** Длина линейки ОЭП, мм. */
+    val dlOep: Double = 162.0,
+)
+
+enum class AngularMotionMode {
+    CONST_ORIENT,
+    AZIMUTH,
+    SMOOTH_SDI,
+}
+
+/**
+ * ИД на построение программы управления угловым движением.
+ * Поля соответствуют sSurveyID из OrbitalMotion, но без Qt-типов и AIST-префиксов.
+ */
+data class SurveyId(
+    /** Признаки включения ОЭП: 1..4. */
+    val oep: List<Boolean> = listOf(false, false, false, false),
+    /** Номер линии визирования: 1..7. */
+    val nlv: Int = 4,
+    /** Примерное интегральное время начала наблюдения, с. */
+    val t: Double = 0.0,
+    /** Координаты точки прицеливания: широта/долгота в радианах, высота в метрах. */
+    val b: Double = 0.0,
+    val l: Double = 0.0,
+    val h: Double = 0.0,
+    /** Длительность включения, с. */
+    val duration: Double = 0.0,
+    /** Массив СДИ. Используется режимами AZIMUTH и SMOOTH_SDI. */
+    val sdi: List<Double> = emptyList(),
+    /** Азимут сканирования, рад. */
+    val azimuth: Double = 0.0,
+    /** Упреждающий угол, рад. */
+    val uprAngle: Double = 0.0,
+    /** Поместить точку прицеливания в центр маршрута. */
+    val pointInCenter: Boolean = false,
+)
+
+/** Результирующие параметры работы ОЭП. */
+data class OepResult(
+    val state: Boolean = false,
+    val tOn: Double = 0.0,
+    val tOff: Double = 0.0,
+)
+
+/** Одна расчетная точка ПУУД. */
+data class AngularMotionPoint(
+    val t: Double,
+    val orbitalPoint: OrbitalPoint,
+    val orientation: Orientation,
+    val groundPoint: BLHPoint?,
+    val omega: Vector3D = Vector3D(),
+    val eps: Vector3D = Vector3D(),
+    val quaternion: Quaternion3D = Quaternion3D(1.0, 0.0, 0.0, 0.0),
+    val wd: List<Vector3D> = List(7) { Vector3D() },
+    val sdi: List<Double> = List(7) { 0.0 },
+)
+
+/** Полный результат расчета режима углового движения. */
+data class AngularMotionResult(
+    val mode: AngularMotionMode,
+    val startTime: Double,
+    val points: List<AngularMotionPoint>,
+    val tauPoints: List<AngularMotionPoint> = emptyList(),
+    val oeps: List<OepResult> = emptyList(),
+)
+
+class AngularMotionCalculationException(message: String, cause: Throwable? = null) : RuntimeException(message, cause)

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

@@ -0,0 +1,208 @@
+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
+import ballistics.utils.math.Quaternion3D
+import ballistics.utils.math.Vector3D
+import kotlin.math.PI
+import kotlin.math.abs
+import kotlin.math.cos
+import kotlin.math.sin
+import kotlin.math.sqrt
+
+/**
+ * Режим азимутального сканирования, порт AzimuthAISTPUUD без AIST-префикса.
+ *
+ * В отличие от старого Qt-кода, расчет получает орбитальные точки через
+ * ballistics.orbitalPoints.timeStepper.AbstractStepper из существующей ballistics-lib.
+ */
+open class AzimuthPUUD(
+    stepper: AbstractStepper,
+    earthType: EarthType = EarthType.PZ90d02,
+    config: AngularMotionConfig = AngularMotionConfig(),
+    private val sickle: Boolean = false,
+) : AbstractPuudCalculator(stepper, earthType, config) {
+
+    override fun calculate(id: SurveyId): AngularMotionResult {
+        validate(id)
+        if (id.sdi.isEmpty() || abs(id.sdi.first()) < 1.0e-4) {
+            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)
+        return AngularMotionResult(
+            mode = AngularMotionMode.AZIMUTH,
+            startTime = tn,
+            points = points,
+            tauPoints = calculateTauPoints(points, id.nlv),
+            oeps = oeps,
+        )
+    }
+
+    protected open fun calculateAzimuth(id: SurveyId, tn: Double, duration: Double): List<AngularMotionPoint> {
+        val step = calculationStep()
+        val routeNormalGsk = routeNormalInGreenwich(id.b, id.l, id.h, id.azimuth)
+        val firstTargetGsk = astro.earth.blh2xyz(id.b, id.l, id.h)
+        var currentSdi = 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)
+        var liv = initialLiv
+
+        val result = mutableListOf<AngularMotionPoint>()
+        var t = tn
+        var elapsed = 0.0
+        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)
+            result += point
+            previous = point
+
+            if (elapsed + step <= duration + EPS) {
+                liv = integrateQuaternionRK4(t, liv, step) { time, q ->
+                    val p = pointAt(time)
+                    val ask = astro.grinvToASK(p)
+                    val di = slantRangeFromQuaternion(q, ask.r)
+                    ownCornerSpeed(time, q, di, ask.r, ask.v, currentSdi)
+                }
+            }
+            elapsed += step
+            t += step
+        }
+
+        return result
+    }
+
+    protected open fun ownCornerSpeed(
+        time: Double,
+        liv: Quaternion3D,
+        di: Vector3D,
+        rAbs: Vector3D,
+        vAbs: Vector3D,
+        sdi: Double,
+    ): Vector3D {
+        val omegaEarth = Vector3D(0.0, 0.0, astro.earth.wEarth)
+        val relVelocityAbs = vAbs - omegaEarth.rem(rAbs)
+        val relVelocityVisir = liv.inverse() * relVelocityAbs
+        val range = di.module().coerceAtLeast(EPS)
+        val vs = relVelocityVisir / range
+        val omegaEarthVisir = liv.inverse() * omegaEarth
+        return if (sickle) {
+            Vector3D(0.0, vs.z + sdi / config.focus + omegaEarthVisir.z, -vs.y + omegaEarthVisir.y)
+        } else {
+            Vector3D(0.0, vs.z + omegaEarthVisir.y, -vs.y - sdi / config.focus + omegaEarthVisir.z)
+        }
+    }
+
+    protected fun buildIntegratedPoint(
+        t: Double,
+        nlv: Int,
+        orientation: Orientation,
+        liv: Quaternion3D,
+        previous: AngularMotionPoint?,
+        sickle: Boolean,
+    ): AngularMotionPoint {
+        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)
+        return AngularMotionPoint(
+            t = t,
+            orbitalPoint = orbital,
+            orientation = orientation,
+            groundPoint = ground,
+            omega = omega,
+            eps = eps,
+            quaternion = liv.normalized(),
+            wd = wd,
+            sdi = sdiForWd(wd, sickle),
+        )
+    }
+
+    protected fun initialVisirQuaternion(
+        rAbs: Vector3D,
+        targetAbs: Vector3D,
+        routeNormalGsk: 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(),
+            )
+        }
+        return quaternionFromMatrix(m).inverse().normalized()
+    }
+
+    protected fun slantRangeFromQuaternion(liv: Quaternion3D, rAbs: Vector3D): Vector3D {
+        val lineAbs = (liv * Vector3D(1.0, 0.0, 0.0)).normSafe()
+        val a = astro.earth.ekvRadius
+        val b = astro.earth.polarRadius
+        val aa = b * b * lineAbs.x * lineAbs.x + b * b * lineAbs.y * lineAbs.y + a * a * lineAbs.z * lineAbs.z
+        val bb = 2.0 * b * b * lineAbs.x * rAbs.x + 2.0 * b * b * lineAbs.y * rAbs.y + 2.0 * a * a * lineAbs.z * rAbs.z
+        val cc = b * b * rAbs.x * rAbs.x + b * b * rAbs.y * rAbs.y + a * a * rAbs.z * rAbs.z - a * a * b * b
+        val root = minPositiveRoot(aa, bb, cc) ?: return Vector3D()
+        return lineAbs * root
+    }
+
+    protected fun orientationFromVisirQuaternion(orbital: ballistics.types.OrbitalPoint, nlv: Int, liv: Quaternion3D): Orientation {
+        val ask = astro.grinvToASK(orbital)
+        val absToOrbit = orbBookToOrbMatrix() * absToOrbBookMatrix(ask.r, ask.v)
+        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 orbitToConnected = connectedToAbs * absToOrbit.transpose()
+        return anglesFromOrbToCon(orbitToConnected)
+    }
+
+    private fun shiftedPairOeps(id: SurveyId, tn: Double, rvo: Double): List<OepResult> {
+        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
+    }
+}

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

@@ -0,0 +1,64 @@
+package space.nstart.pcp.angularmotion
+
+import ballistics.orbitalPoints.timeStepper.AbstractStepper
+import ballistics.types.EarthType
+import ballistics.types.Orientation
+
+/**
+ * Режим постоянной ориентации, порт ConstOrientAISTPUUD без AIST-префикса.
+ *
+ * Базовая логика повторяет исходный класс: уточняется время начала по упреждающему
+ * углу, затем с дискретом stepPuud рассчитываются углы ориентации, точка пересечения
+ * ЛВ с Землей, кватернион, угловая скорость, W/D и СДИ по всем семи ЛВ.
+ */
+class ConstOrientPUUD(
+    stepper: AbstractStepper,
+    earthType: EarthType = EarthType.PZ90d02,
+    config: AngularMotionConfig = AngularMotionConfig(),
+) : AbstractPuudCalculator(stepper, earthType, config) {
+
+    override fun calculate(id: SurveyId): AngularMotionResult {
+        validate(id)
+        val tn = calcTn(id)
+        val points = calculateConst(id, tn, id.duration)
+        return AngularMotionResult(
+            mode = AngularMotionMode.CONST_ORIENT,
+            startTime = tn,
+            points = points,
+            tauPoints = calculateTauPoints(points, id.nlv),
+            oeps = buildOeps(id, tn),
+        )
+    }
+
+    private fun calculateConst(id: SurveyId, tn: Double, duration: Double): List<AngularMotionPoint> {
+        val result = mutableListOf<AngularMotionPoint>()
+        val step = calculationStep()
+        var t = tn
+        var elapsed = 0.0
+        var previous: AngularMotionPoint? = null
+        var b = id.b
+        var l = id.l
+        var h = 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 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
+    }
+}

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

@@ -0,0 +1,216 @@
+package space.nstart.pcp.angularmotion
+
+import ballistics.types.Orientation
+import ballistics.utils.math.Matrix3D
+import ballistics.utils.math.Quaternion3D
+import ballistics.utils.math.Vector3D
+import kotlin.math.PI
+import kotlin.math.abs
+import kotlin.math.acos
+import kotlin.math.atan2
+import kotlin.math.cos
+import kotlin.math.floor
+import kotlin.math.max
+import kotlin.math.min
+import kotlin.math.sin
+import kotlin.math.sqrt
+
+internal const val EPS = 1.0e-12
+
+internal fun Vector3D.normSafe(): Vector3D {
+    val m = module()
+    return if (m < EPS || !m.isFinite()) Vector3D() else Vector3D(x / m, y / m, z / m)
+}
+
+internal operator fun Vector3D.div(k: Double): Vector3D = Vector3D(x / k, y / k, z / k)
+
+internal fun Vector3D.copyOf(): Vector3D = Vector3D(x, y, z)
+
+internal fun Quaternion3D.copyOf(): Quaternion3D = Quaternion3D(q0, q1, q2, q3)
+
+internal fun Quaternion3D.normalized(): Quaternion3D {
+    val n = norm()
+    return if (n < EPS || !n.isFinite()) Quaternion3D(1.0, 0.0, 0.0, 0.0) else Quaternion3D(q0 / n, q1 / n, q2 / n, q3 / n)
+}
+
+internal fun Quaternion3D.inverse(): Quaternion3D = normalized().opposite()
+
+internal fun Quaternion3D.scaled(k: Double): Quaternion3D = Quaternion3D(q0 * k, q1 * k, q2 * k, q3 * k)
+
+internal fun Matrix3D.copyOf(): Matrix3D = Matrix3D(first.copyOf(), second.copyOf(), third.copyOf())
+
+internal fun orientationMatrix(orientation: Orientation): Matrix3D {
+    val tang = orientation.tang
+    val kren = orientation.kren
+    val risk = orientation.risk
+    return Matrix3D(
+        Vector3D(
+            cos(tang) * cos(risk) - sin(tang) * sin(kren) * sin(risk),
+            -sin(tang) * cos(kren),
+            cos(tang) * sin(risk) + sin(tang) * sin(kren) * cos(risk),
+        ),
+        Vector3D(
+            sin(tang) * cos(risk) + cos(tang) * sin(kren) * sin(risk),
+            cos(tang) * cos(kren),
+            sin(tang) * sin(risk) - cos(tang) * sin(kren) * cos(risk),
+        ),
+        Vector3D(
+            -cos(kren) * sin(risk),
+            sin(kren),
+            cos(kren) * cos(risk),
+        ),
+    )
+}
+
+internal fun anglesFromOrbToCon(m: Matrix3D): Orientation {
+    val mt = m.transpose()
+    var kren = asinClamped(mt.third.y)
+    val cosKren = cos(kren).coerceAwayFromZero()
+    var tang = atan2(-mt.first.y / cosKren, (mt.second.y / cosKren).coerceIn(-1.0, 1.0))
+    if (tang > PI) tang -= 2.0 * PI
+    var risk = atan2(-mt.third.x / cosKren, mt.third.z / cosKren)
+    if (risk > PI) risk -= 2.0 * PI
+    return Orientation(normalizeAngle(tang), normalizeAngle(kren), normalizeAngle(risk))
+}
+
+internal fun asinClamped(v: Double): Double = kotlin.math.asin(v.coerceIn(-1.0, 1.0))
+
+internal fun Double.coerceAwayFromZero(): Double =
+    when {
+        abs(this) >= EPS -> this
+        this < 0.0 -> -EPS
+        else -> EPS
+    }
+
+internal fun normalizeAngle(value: Double): Double {
+    var x = value
+    while (x > PI) x -= 2.0 * PI
+    while (x < -PI) x += 2.0 * PI
+    return x
+}
+
+internal fun normalizePositiveAngle(value: Double): Double {
+    var x = value
+    while (x < 0.0) x += 2.0 * PI
+    while (x >= 2.0 * PI) x -= 2.0 * PI
+    return x
+}
+
+internal fun rotationZ(angle: Double): Matrix3D = Matrix3D().also { it.makeOzMatrix(angle) }
+
+internal fun rotationX(angle: Double): Matrix3D = Matrix3D().also { it.makeOxMatrix(angle) }
+
+internal fun rotationY(angle: Double): Matrix3D = Matrix3D().also { it.makeOyMatrix(angle) }
+
+/**
+ * Матрица перехода от Абсолютной СК к орбитальной книжной СК по той же схеме,
+ * которая уже используется в ballistics.mpl.OrientOnPointCalculator.
+ */
+internal fun absToOrbBookMatrix(rAbs: Vector3D, vAbs: Vector3D): Matrix3D {
+    val c = Matrix3D()
+    c.second = rAbs.normSafe()
+    c.third = (vAbs.rem(rAbs)).normSafe()
+    c.first = c.second.rem(c.third).normSafe()
+    return c
+}
+
+/** Матрица из OrbitalMotion::TLowOrbit::OrbBookToOrb. */
+internal fun orbBookToOrbMatrix(): Matrix3D = Matrix3D(
+    Vector3D(1.0, 0.0, 0.0),
+    Vector3D(0.0, 0.0, 1.0),
+    Vector3D(0.0, -1.0, 0.0),
+)
+
+internal fun conToVisirMatrix(): Matrix3D = Matrix3D(
+    Vector3D(0.0, -1.0, 0.0),
+    Vector3D(-1.0, 0.0, 0.0),
+    Vector3D(0.0, 0.0, -1.0),
+)
+
+internal fun conToOpticMatrix(): Matrix3D = Matrix3D(
+    Vector3D(0.0, 1.0, 0.0),
+    Vector3D(1.0, 0.0, 0.0),
+    Vector3D(0.0, 0.0, -1.0),
+)
+
+internal fun lineOfSightVectorInConnected(lineAngle: Double): Vector3D {
+    // Центральная ЛВ в существующей ballistics-lib соответствует (0, 1, 0).
+    // Отклонение ЛВ переносится как поворот вокруг OZ связанной СК.
+    return Vector3D(-sin(lineAngle), cos(lineAngle), 0.0).normSafe()
+}
+
+internal fun quaternionFromMatrix(m: Matrix3D): Quaternion3D = Quaternion3D().also { it.fromMatrixStanley(m) }.normalized()
+
+internal fun quaternionFromEuler(tang: Double, kren: Double, risk: Double): Quaternion3D =
+    quaternionFromMatrix(orientationMatrix(Orientation(tang, kren, risk))).normalized()
+
+internal fun omegaFromTwoQuat(q1: Quaternion3D, q2: Quaternion3D, step: Double): Vector3D {
+    if (abs(step) < EPS) return Vector3D()
+    val q1n = q1.normalized()
+    val dq = (q2.normalized() - q1n).scaled(1.0 / step)
+    val l1 = Vector3D(q1n.q1, q1n.q2, q1n.q3)
+    val l2 = Vector3D(dq.q1, dq.q2, dq.q3)
+    return (l1 * (-dq.q0) + l2 * q1n.q0 - (l1.rem(l2))) * 2.0
+}
+
+internal fun integrateQuaternionRK4(
+    time: Double,
+    q: Quaternion3D,
+    step: Double,
+    omegaProvider: (Double, Quaternion3D) -> Vector3D,
+): Quaternion3D {
+    fun derivative(t: Double, qq: Quaternion3D): Quaternion3D {
+        val n2 = qq.q0 * qq.q0 + qq.q1 * qq.q1 + qq.q2 * qq.q2 + qq.q3 * qq.q3
+        val omega = omegaProvider(t, qq.normalized())
+        return Quaternion3D(
+            (-omega.x * qq.q1 - omega.y * qq.q2 - omega.z * qq.q3 - qq.q0 * (n2 - 1.0)) / 2.0,
+            (omega.x * qq.q0 + omega.z * qq.q2 - omega.y * qq.q3 - qq.q1 * (n2 - 1.0)) / 2.0,
+            (omega.y * qq.q0 + omega.x * qq.q3 - omega.z * qq.q1 - qq.q2 * (n2 - 1.0)) / 2.0,
+            (omega.z * qq.q0 + omega.y * qq.q1 - omega.x * qq.q2 - qq.q3 * (n2 - 1.0)) / 2.0,
+        )
+    }
+
+    val k1 = derivative(time, q)
+    val k2 = derivative(time + step / 2.0, q + k1.scaled(step / 2.0))
+    val k3 = derivative(time + step / 2.0, q + k2.scaled(step / 2.0))
+    val k4 = derivative(time + step, q + k3.scaled(step))
+
+    return (q + (k1 + k2.scaled(2.0) + k3.scaled(2.0) + k4).scaled(step / 6.0)).normalized()
+}
+
+internal fun roundDownByDay(value: Double, step: Double): Double {
+    if (step <= 0.0) return value
+    val day = floor(value / 86400.0) * 86400.0
+    val seconds = value - day
+    return day + floor(seconds / step) * step
+}
+
+internal fun lagrange(x: Double, xs: List<Double>, ys: List<Double>): Double {
+    require(xs.size == ys.size && xs.isNotEmpty())
+    var result = 0.0
+    for (i in xs.indices) {
+        var term = ys[i]
+        for (j in xs.indices) {
+            if (i != j) {
+                val denom = xs[i] - xs[j]
+                if (abs(denom) >= EPS) {
+                    term *= (x - xs[j]) / denom
+                }
+            }
+        }
+        result += term
+    }
+    return result
+}
+
+internal fun minPositiveRoot(a: Double, b: Double, c: Double): Double? {
+    val d = b * b - 4.0 * a * c
+    if (d < 0.0 || abs(a) < EPS) return null
+    val sqrtD = sqrt(d)
+    val x1 = (-b + sqrtD) / (2.0 * a)
+    val x2 = (-b - sqrtD) / (2.0 * a)
+    val candidates = listOf(x1, x2).filter { it.isFinite() && it > 0.0 }
+    return candidates.minOrNull()
+}
+
+internal fun clampUnit(v: Double): Double = max(-1.0, min(1.0, v))

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

@@ -0,0 +1,83 @@
+package space.nstart.pcp.angularmotion
+
+import ballistics.orbitalPoints.timeStepper.AbstractStepper
+import ballistics.types.EarthType
+import ballistics.utils.math.Matrix3D
+import ballistics.utils.math.Quaternion3D
+import ballistics.utils.math.Vector3D
+
+/**
+ * Режим плавной реализации СДИ, порт SmoothSDIAISTPUUD без AIST-префикса.
+ *
+ * Режим наследует азимутальную постановку маршрута, но переопределяет расчет
+ * абсолютной угловой скорости в визирной СК по схеме OwnCornerSpeedLevelSpeed.
+ */
+class SmoothSDIPUUD(
+    stepper: AbstractStepper,
+    earthType: EarthType = EarthType.PZ90d02,
+    config: AngularMotionConfig = AngularMotionConfig(),
+) : AzimuthPUUD(stepper, earthType, config, sickle = false) {
+
+    override fun calculate(id: SurveyId): AngularMotionResult {
+        validate(id)
+        if (id.sdi.isEmpty()) {
+            throw AngularMotionCalculationException("Для режима SmoothSDI требуется массив СДИ")
+        }
+        val tn = calcTn(id)
+        val points = calculateAzimuth(id, tn, id.duration)
+        return AngularMotionResult(
+            mode = AngularMotionMode.SMOOTH_SDI,
+            startTime = tn,
+            points = points,
+            tauPoints = calculateTauPoints(points, id.nlv),
+            oeps = buildOeps(id, tn),
+        )
+    }
+
+    override fun ownCornerSpeed(
+        time: Double,
+        liv: Quaternion3D,
+        di: Vector3D,
+        rAbs: Vector3D,
+        vAbs: Vector3D,
+        sdi: Double,
+    ): Vector3D {
+        val omegaEarth = Vector3D(0.0, 0.0, astro.earth.wEarth)
+        val relVelocityAbs = vAbs - omegaEarth.rem(rAbs)
+
+        val visToCon = conToVisirMatrix().transpose()
+        val visToOptic = visToCon * conToOpticMatrix().transpose()
+        val velocityOptic = visToOptic * (liv.inverse() * relVelocityAbs)
+        val range = di.module().coerceAtLeast(EPS)
+        val vsOptic = velocityOptic / range
+
+        val miv = liv.matrix().transpose()
+        val currentAbs = rAbs + di
+        val currentGsk = astro.askToGrinvich(currentAbs, time)
+        val mih = horizontalGeodesicToAbs(currentGsk, time)
+        val chv = miv * mih.transpose()
+        val perspective = visToOptic * chv
+        val denom = perspective.first.x.coerceAwayFromZero()
+
+        val programmed = Vector3D(
+            -vsOptic.y * perspective.third.x / denom,
+            -vsOptic.z,
+            vsOptic.y - sdi / config.focus,
+        )
+        val omegaInVisir = visToOptic * programmed + (liv.inverse() * omegaEarth)
+        return omegaInVisir
+    }
+
+    private fun horizontalGeodesicToAbs(pointGsk: Vector3D, time: Double): Matrix3D {
+        val blh = astro.earth.xyz2blh(pointGsk)
+        val upGsk = pointGsk.normSafe()
+        val eastGsk = Vector3D(-kotlin.math.sin(blh.long), kotlin.math.cos(blh.long), 0.0).normSafe()
+        val northGsk = upGsk.rem(eastGsk).normSafe()
+        val gskToAbs = Matrix3D().also { it.makeOzMatrix(astro.si2000(time)) }
+        return Matrix3D(
+            gskToAbs * northGsk,
+            gskToAbs * eastGsk,
+            gskToAbs * upGsk,
+        )
+    }
+}

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

@@ -0,0 +1,74 @@
+package space.nstart.pcp.angularmotion
+
+import ballistics.orbitalPoints.timeStepper.AbstractStepper
+import ballistics.types.OrbitalPoint
+import ballistics.utils.math.Vector3D
+import kotlin.math.cos
+import kotlin.math.sin
+import kotlin.test.Test
+import kotlin.test.assertEquals
+import kotlin.test.assertTrue
+
+class AngularMotionCalculatorSmokeTest {
+    @Test
+    fun `const orientation returns points`() {
+        val result = ConstOrientPUUD(CircularStepper()).calculate(
+            SurveyId(
+                nlv = 4,
+                t = 1000.0,
+                b = 0.1,
+                l = 0.2,
+                h = 0.0,
+                duration = 1.0,
+            )
+        )
+
+        assertEquals(AngularMotionMode.CONST_ORIENT, result.mode)
+        assertTrue(result.points.isNotEmpty())
+    }
+
+    @Test
+    fun `factory creates smooth sdi calculator`() {
+        val calculator = AngularMotionCalculatorFactory.create(
+            mode = AngularMotionMode.SMOOTH_SDI,
+            stepper = CircularStepper(),
+        )
+
+        val result = calculator.calculate(
+            SurveyId(
+                nlv = 4,
+                t = 1000.0,
+                b = 0.1,
+                l = 0.2,
+                h = 0.0,
+                duration = 1.0,
+                sdi = listOf(100.0),
+                azimuth = 0.3,
+            )
+        )
+
+        assertEquals(AngularMotionMode.SMOOTH_SDI, result.mode)
+        assertTrue(result.points.isNotEmpty())
+    }
+
+    private class CircularStepper : AbstractStepper {
+        private val radius = 7_000_000.0
+        private val omega = 0.001
+
+        override fun calculate(t: Double): OrbitalPoint = point(t)
+
+        override fun calculate(t: Double, p: OrbitalPoint): OrbitalPoint = point(t)
+
+        override fun clear() = Unit
+
+        private fun point(t: Double): OrbitalPoint {
+            val a = omega * t
+            return OrbitalPoint(
+                t = t,
+                vit = 0,
+                r = Vector3D(radius * cos(a), radius * sin(a), radius * 0.01 * sin(a * 0.5)),
+                v = Vector3D(-radius * omega * sin(a), radius * omega * cos(a), radius * 0.005 * omega * cos(a * 0.5)),
+            )
+        }
+    }
+}