ivan-igorevich/dc-observatio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ПУУД с постоянным азимутом

Browse Source
This commit is contained in:
emelianov
2026-06-09 13:41:50 +03:00
parent faf88eaf94
commit e6822b6caf
2 changed files with 23 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt
+16 -8
View File
@@ -69,18 +69,22 @@ open class AzimuthPUUD(
while (elapsed <= duration + EPS) { while (elapsed <= duration + EPS) {
val orbital = pointAt(t) val orbital = pointAt(t)
val ask = astro.grinvToASK(orbital)
val di = slantRangeFromQuaternion(liv, ask.r)
val sdiForTime = sdiAt(id.sdi, elapsed)
val omegaVisir = ownCornerSpeed(t, liv, di, ask.r, ask.v, sdiForTime)
val orientation = orientationFromVisirQuaternion(orbital, liv) val orientation = orientationFromVisirQuaternion(orbital, liv)
val point = buildIntegratedPoint(t, orientation, liv, previous, sickle) val point = buildIntegratedPoint(t, orientation, omegaVisir, previous, sickle)
result += point result += point
previous = point previous = point
if (elapsed + step <= duration + EPS) { if (elapsed + step <= duration + EPS) {
liv = integrateQuaternionRK4(t, liv, step) { time, q -> liv = integrateQuaternionRK4(t, liv, step) { time, q ->
val p = pointAt(time) val p = pointAt(time)
val ask = astro.grinvToASK(p) val pAsk = astro.grinvToASK(p)
val di = slantRangeFromQuaternion(q, ask.r) val pDi = slantRangeFromQuaternion(q, pAsk.r)
val sdiForTime = sdiAt(id.sdi, time - tn) val pSdi = sdiAt(id.sdi, time - tn)
ownCornerSpeed(time, q, di, ask.r, ask.v, sdiForTime) ownCornerSpeed(time, q, pDi, pAsk.r, pAsk.v, pSdi)
} }
} }
elapsed += step elapsed += step
@@ -114,14 +118,15 @@ open class AzimuthPUUD(
protected fun buildIntegratedPoint( protected fun buildIntegratedPoint(
t: Double, t: Double,
orientation: Orientation, orientation: Orientation,
liv: Quaternion3D, omegaVisir: Vector3D,
previous: AngularMotionPoint?, previous: AngularMotionPoint?,
sickle: Boolean, sickle: Boolean,
): AngularMotionPoint { ): AngularMotionPoint {
val orbital = pointAt(t) val orbital = pointAt(t)
val omega = previous?.let { omegaFromTwoQuat(it.quaternion, liv, t - it.t) } ?: Vector3D() val omega = visirToConnected(omegaVisir)
val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D() 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 ground = pointOnEarth(orbital, orientation)
val q = quaternionFor(orbital, orientation)
val wd = wd(orbital, orientation, omega) val wd = wd(orbital, orientation, omega)
return AngularMotionPoint( return AngularMotionPoint(
t = t, t = t,
@@ -130,12 +135,15 @@ open class AzimuthPUUD(
groundPoint = ground, groundPoint = ground,
omega = omega, omega = omega,
eps = eps, eps = eps,
quaternion = liv.normalized(), quaternion = q,
wd = wd, wd = wd,
sdi = sdiForWd(wd, sickle), sdi = sdiForWd(wd, sickle),
) )
} }
private fun visirToConnected(omegaVisir: Vector3D): Vector3D =
conToVisirMatrix().transpose() * omegaVisir
protected fun initialVisirQuaternion( protected fun initialVisirQuaternion(
rAbs: Vector3D, rAbs: Vector3D,
targetAbs: Vector3D, targetAbs: Vector3D,
libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt
+7 -1
View File
@@ -200,13 +200,19 @@ class AngularMotionCalculatorSmokeTest {
println( println(
String.format( String.format(
Locale.US, Locale.US,
"%s %.3f %.3f %.3f , %s %s", "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f",
toDateTime(v.t).format(outputTimeFormatter), toDateTime(v.t).format(outputTimeFormatter),
v.orientation.tang * 180 / PI, v.orientation.tang * 180 / PI,
v.orientation.kren * 180 / PI, v.orientation.kren * 180 / PI,
v.orientation.risk * 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.lat * 180 / PI) },
v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) }, v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) },
v.omega.x,
v.omega.y,
v.omega.z,
v.wd.x,
v.wd.y,
v.wd.z
) )
) )
} }