Расчет с выравниванием СДИ

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

@@ -201,6 +201,28 @@ abstract class AbstractPuudCalculator(
     protected fun sdiForWd(wd: Vector3D, sickle: Boolean = false): Double =
         (if (sickle) wd.z else wd.x) * config.focus
 
+    protected fun edgeLineOfSightVectorsInConnected(): Pair<Vector3D, Vector3D>? {
+        if (config.focus <= EPS || config.dlOep <= EPS) return null
+        val leftOffsetMm = config.dxOep - config.dlOep / 2.0
+        val rightOffsetMm = config.dxOep + config.dlOep / 2.0
+        return lineOfSightByFocalPlaneZ(leftOffsetMm) to lineOfSightByFocalPlaneZ(rightOffsetMm)
+    }
+
+    protected fun edgeSdiValues(
+        point: OrbitalPoint,
+        orientation: Orientation,
+        omegaConnected: Vector3D,
+        sickle: Boolean = false,
+    ): Pair<Double, Double>? {
+        val (leftLine, rightLine) = edgeLineOfSightVectorsInConnected() ?: return null
+        val leftSdi = sdiForWd(wd(point, orientation, omegaConnected, leftLine), sickle)
+        val rightSdi = sdiForWd(wd(point, orientation, omegaConnected, rightLine), sickle)
+        return leftSdi to rightSdi
+    }
+
+    private fun lineOfSightByFocalPlaneZ(offsetMm: Double): Vector3D =
+        Vector3D(0.0, -config.focus, offsetMm).normSafe()
+
     /**
      * Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для единственной центральной линии визирования.
      *
@@ -209,16 +231,24 @@ abstract class AbstractPuudCalculator(
      * добавляется радиальная составляющая изменения наклонной дальности, чтобы точка оставалась
      * на поверхности эллипсоида. Такая схема соответствует формулам из OrbitalMotion::AbstractAISTPUUD::wd.
      */
-    protected fun wd(point: OrbitalPoint, orientation: Orientation, omegaConnected: Vector3D): Vector3D {
+    protected fun wd(point: OrbitalPoint, orientation: Orientation, omegaConnected: Vector3D): Vector3D =
+        wd(point, orientation, omegaConnected, lineOfSightVectorInConnected())
+
+    protected fun wd(
+        point: OrbitalPoint,
+        orientation: Orientation,
+        omegaConnected: Vector3D,
+        lineConnected: Vector3D,
+    ): Vector3D {
         val ask = astro.grinvToASK(point)
-        val lineConnected = lineOfSightVectorInConnected()
+        val lineInConnected = lineConnected.normSafe()
         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()
+        val lineGsk = (absToGsk * orbitBookToAbs * orbitToOrbitBook * connectedToOrbit * lineInConnected).normSafe()
         if (lineGsk.module() < EPS) return Vector3D()
 
         val earth = astro.earth
@@ -310,6 +340,9 @@ abstract class AbstractPuudCalculator(
         val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D()
         val ground = pointOnEarth(orbital, orientation)
         val wd = wd(orbital, orientation, omega)
+        val edgeSdi = edgeSdiValues(orbital, orientation, omega, sickle)
+        val centerSdi = sdiForWd(wd, sickle)
+        val sdiSpread = edgeSdi?.let { maxOf(abs(it.first - centerSdi), abs(it.second - centerSdi)) }
         return AngularMotionPoint(
             t = t,
             orbitalPoint = orbital,
@@ -319,7 +352,10 @@ abstract class AbstractPuudCalculator(
             eps = eps,
             quaternion = q,
             wd = wd,
-            sdi = sdiForWd(wd, sickle),
+            sdi = centerSdi,
+            sdiLeft = edgeSdi?.first,
+            sdiRight = edgeSdi?.second,
+            sdiSpread = sdiSpread,
         )
     }
 

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

@@ -76,6 +76,12 @@ data class AngularMotionPoint(
     val quaternion: Quaternion3D = Quaternion3D(1.0, 0.0, 0.0, 0.0),
     val wd: Vector3D = Vector3D(),
     val sdi: Double = 0.0,
+    /** СДИ на левом краю линейки ОЭП, мм/с. */
+    val sdiLeft: Double? = null,
+    /** СДИ на правом краю линейки ОЭП, мм/с. */
+    val sdiRight: Double? = null,
+    /** Максимальное отклонение краевой СДИ от центральной, мм/с. */
+    val sdiSpread: Double? = null,
 )
 
 /** Полный результат расчета режима углового движения. */

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

@@ -152,6 +152,9 @@ open class AzimuthPUUD(
         val ground = pointOnEarth(orbital, orientation)
         val q = quaternionFor(orbital, orientation)
         val wd = wd(orbital, orientation, omega)
+        val edgeSdi = edgeSdiValues(orbital, orientation, omega, sickle)
+        val centerSdi = sdiForWd(wd, sickle)
+        val sdiSpread = edgeSdi?.let { maxOf(abs(it.first - centerSdi), abs(it.second - centerSdi)) }
         return AngularMotionPoint(
             t = t,
             orbitalPoint = orbital,
@@ -161,11 +164,14 @@ open class AzimuthPUUD(
             eps = eps,
             quaternion = q,
             wd = wd,
-            sdi = sdiForWd(wd, sickle),
+            sdi = centerSdi,
+            sdiLeft = edgeSdi?.first,
+            sdiRight = edgeSdi?.second,
+            sdiSpread = sdiSpread,
         )
     }
 
-    private fun visirToConnected(omegaVisir: Vector3D): Vector3D =
+    protected fun visirToConnected(omegaVisir: Vector3D): Vector3D =
         conToVisirMatrix().transpose() * omegaVisir
 
     protected fun initialVisirQuaternion(

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

@@ -66,7 +66,47 @@ class SmoothSDIPUUD(
             vsOptic.y - sdi / config.focus,
         )
         val omegaInVisir = visToOptic * programmed + (liv.inverse() * omegaEarth)
-        return omegaInVisir
+        return alignEdgeSdi(time, liv, omegaInVisir, sdi)
+    }
+
+    /**
+     * Вариант №3 требует не только Wz/D = 0 и заданную СДИ на центральной ЦЛВ,
+     * но и минимизацию изменения СДИ по ширине полосы. Центральная ЦЛВ не чувствительна
+     * к вращению вокруг себя, зато крайние лучи линейки ОЭП чувствительны. Поэтому
+     * подбираем добавку к компоненте угловой скорости вокруг ЦЛВ так, чтобы СДИ на
+     * левом и правом краях была максимально близка к заданной.
+     */
+    private fun alignEdgeSdi(
+        time: Double,
+        liv: Quaternion3D,
+        omegaInVisir: Vector3D,
+        targetSdi: Double,
+    ): Vector3D {
+        if (config.dlOep <= EPS || config.focus <= EPS) return omegaInVisir
+
+        val orbital = pointAt(time)
+        val orientation = orientationFromVisirQuaternion(orbital, liv)
+
+        fun edgeSdiFor(omegaVisir: Vector3D): Pair<Double, Double>? =
+            edgeSdiValues(orbital, orientation, visirToConnected(omegaVisir), sickle = false)
+
+        val baseEdges = edgeSdiFor(omegaInVisir) ?: return omegaInVisir
+        val probeOmega = Vector3D(omegaInVisir.x + LINE_SPIN_PROBE, omegaInVisir.y, omegaInVisir.z)
+        val probeEdges = edgeSdiFor(probeOmega) ?: return omegaInVisir
+
+        val leftK = (probeEdges.first - baseEdges.first) / LINE_SPIN_PROBE
+        val rightK = (probeEdges.second - baseEdges.second) / LINE_SPIN_PROBE
+        val denom = leftK * leftK + rightK * rightK
+        if (denom < EPS || !denom.isFinite()) return omegaInVisir
+
+        val correction = -((baseEdges.first - targetSdi) * leftK + (baseEdges.second - targetSdi) * rightK) / denom
+        if (!correction.isFinite()) return omegaInVisir
+
+        return Vector3D(
+            omegaInVisir.x + correction.coerceIn(-MAX_LINE_SPIN_CORRECTION, MAX_LINE_SPIN_CORRECTION),
+            omegaInVisir.y,
+            omegaInVisir.z,
+        )
     }
 
     private fun horizontalGeodesicToAbs(
@@ -93,4 +133,9 @@ class SmoothSDIPUUD(
             gskToAbs * upGsk,
         )
     }
+    private companion object {
+        const val LINE_SPIN_PROBE = 1.0e-5
+        const val MAX_LINE_SPIN_CORRECTION = 0.05
+    }
+
 }