From 78887dd63a10aaadc0d7fa870c785e0da8bfc02c Mon Sep 17 00:00:00 2001 From: emelianov Date: Mon, 1 Jun 2026 14:22:36 +0300 Subject: [PATCH 1/3] fix requests --- config-repo/pcp-request-service.yaml | 11 ++++++ .../RequestServiceConfigTest.kt | 36 +++++++++++++++++++ 2 files changed, 47 insertions(+) create mode 100644 services/pcp-request-service/src/test/kotlin/org/nstart/dep265/requestservice/RequestServiceConfigTest.kt diff --git a/config-repo/pcp-request-service.yaml b/config-repo/pcp-request-service.yaml index ef094a1..6f2e83e 100644 --- a/config-repo/pcp-request-service.yaml +++ b/config-repo/pcp-request-service.yaml @@ -67,3 +67,14 @@ app: cleanup-cron: "0 0 3 * * *" grid: enabled: true + +--- +spring: + config: + activate: + on-profile: local + datasource: + driver-class-name: org.postgresql.Driver + url: jdbc:postgresql://${pcp.infra.postgres.host}:${pcp.infra.postgres.port}/pcp_requests + username: postgres + password: password diff --git a/services/pcp-request-service/src/test/kotlin/org/nstart/dep265/requestservice/RequestServiceConfigTest.kt b/services/pcp-request-service/src/test/kotlin/org/nstart/dep265/requestservice/RequestServiceConfigTest.kt new file mode 100644 index 0000000..cb0f87a --- /dev/null +++ b/services/pcp-request-service/src/test/kotlin/org/nstart/dep265/requestservice/RequestServiceConfigTest.kt @@ -0,0 +1,36 @@ +package org.nstart.dep265.requestservice + +import java.nio.file.Files +import java.nio.file.Path +import kotlin.io.path.readText +import kotlin.test.Test +import kotlin.test.assertTrue + +class RequestServiceConfigTest { + @Test + fun `local profile uses concrete postgres datasource`() { + val config = configRepositoryFile().readText() + + assertTrue(config.contains("on-profile: local")) + assertTrue(config.contains("url: jdbc:postgresql://\${pcp.infra.postgres.host}:\${pcp.infra.postgres.port}/pcp_requests")) + assertTrue(config.contains("username: postgres")) + assertTrue(config.contains("password: password")) + } + + @Test + fun `shared datasource keeps environment placeholders for deployed profiles`() { + val config = configRepositoryFile().readText() + + assertTrue(config.contains("url: \"\${SPRING_DATASOURCE_URL}\"")) + assertTrue(config.contains("username: \"\${SPRING_DATASOURCE_USERNAME}\"")) + assertTrue(config.contains("password: \"\${SPRING_DATASOURCE_PASSWORD}\"")) + } + + private fun configRepositoryFile(): Path { + val rootPath = Path.of("config-repo/pcp-request-service.yaml") + if (Files.exists(rootPath)) { + return rootPath + } + return Path.of("../../config-repo/pcp-request-service.yaml") + } +} From faedd21f43887fb060d03e45566cdf4ea6654fbb Mon Sep 17 00:00:00 2001 From: emelianov Date: Thu, 4 Jun 2026 09:39:27 +0300 Subject: [PATCH 2/3] =?UTF-8?q?=D1=80=D0=B0=D1=81=D1=87=D0=B5=D1=82=20?= =?UTF-8?q?=D0=9F=D0=A3=D0=A3=D0=94?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libs/angular-motion-lib/README.md | 46 +++ libs/angular-motion-lib/build.gradle.kts | 33 ++ .../angularmotion/AbstractPuudCalculator.kt | 308 ++++++++++++++++++ .../angularmotion/AngularMotionCalculator.kt | 6 + .../AngularMotionCalculatorFactory.kt | 18 + .../pcp/angularmotion/AngularMotionTypes.kt | 105 ++++++ .../nstart/pcp/angularmotion/AzimuthPUUD.kt | 208 ++++++++++++ .../pcp/angularmotion/ConstOrientPUUD.kt | 64 ++++ .../nstart/pcp/angularmotion/InternalMath.kt | 216 ++++++++++++ .../nstart/pcp/angularmotion/SmoothSDIPUUD.kt | 83 +++++ .../AngularMotionCalculatorSmokeTest.kt | 74 +++++ settings.gradle.kts | 1 + 12 files changed, 1162 insertions(+) create mode 100644 libs/angular-motion-lib/README.md create mode 100644 libs/angular-motion-lib/build.gradle.kts create mode 100644 libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt create mode 100644 libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculator.kt create mode 100644 libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorFactory.kt create mode 100644 libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionTypes.kt create mode 100644 libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt create mode 100644 libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt create mode 100644 libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt create mode 100644 libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SmoothSDIPUUD.kt create mode 100644 libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt diff --git a/libs/angular-motion-lib/README.md b/libs/angular-motion-lib/README.md new file mode 100644 index 0000000..7da18ea --- /dev/null +++ b/libs/angular-motion-lib/README.md @@ -0,0 +1,46 @@ +# angular-motion-lib + +Библиотека расчета программ управления угловым движением КА. + +Библиотека создана как перенос расчетного контура из проекта `OrbitalMotion/AISTPUUD/SurveyPUUD`: + +- `ConstOrientAISTPUUD` → `ConstOrientPUUD`; +- `AzimuthAISTPUUD` → `AzimuthPUUD`; +- `SmoothSDIAISTPUUD` → `SmoothSDIPUUD`. + +В новых классах нет префикса `AIST`. Вместо старого C++/Qt `Ballistics` используется существующая Kotlin-библиотека проекта PCP: + +- `ballistics.orbitalPoints.timeStepper.AbstractStepper` для выхода на заданное время; +- `ballistics.types.OrbitalPoint`, `Orientation`, `BLHPoint`; +- `ballistics.utils.astro.AstronomerJ2000`; +- `ballistics.utils.math.Vector3D`, `Matrix3D`, `Quaternion3D`. + +Основной API: + +```kotlin +val calculator = AngularMotionCalculatorFactory.create( + mode = AngularMotionMode.AZIMUTH, + stepper = stepper, +) + +val result = calculator.calculate( + SurveyId( + nlv = 4, + t = startTimeSeconds, + b = latitudeRad, + l = longitudeRad, + h = heightMeters, + duration = 20.0, + sdi = listOf(150.0), + azimuth = azimuthRad, + ) +) +``` + +## Примечания по переносу + +1. Временные параметры задаются в интегральных секундах, как в `ballistics-lib`. +2. Углы задаются в радианах. +3. Линейные параметры оптики в `AngularMotionConfig` сохранены в миллиметрах, как в исходном `sBLS_KA`. +4. Расчет времени начала режима сохраняет подход `calcTn`: поиск корня на интервале `t ± 180 с` и округление к `discreteTimes`. +5. Интервал успокоения рассчитывается обратной интерполяцией первых точек ПУУД, аналогично `TauIntervalCalculator`. diff --git a/libs/angular-motion-lib/build.gradle.kts b/libs/angular-motion-lib/build.gradle.kts new file mode 100644 index 0000000..c02af40 --- /dev/null +++ b/libs/angular-motion-lib/build.gradle.kts @@ -0,0 +1,33 @@ +plugins { + kotlin("jvm") + id("io.spring.dependency-management") + jacoco +} + +group = "space.nstart.pcp" +version = "1.0.0" + +kotlin { + jvmToolchain((property("versions.java") as String).toInt()) + compilerOptions { + freeCompilerArgs.addAll("-Xjsr305=strict") + jvmTarget.set(org.jetbrains.kotlin.gradle.dsl.JvmTarget.fromTarget(property("versions.java") as String)) + } +} + +dependencies { + api(project(":libs:ballistics-lib")) + + testImplementation("org.jetbrains.kotlin:kotlin-test-junit5") + testRuntimeOnly("org.junit.platform:junit-platform-launcher") +} + +dependencyManagement { + imports { + mavenBom("org.springframework.boot:spring-boot-dependencies:${property("versions_spring_boot")}") + } +} + +tasks.withType { + useJUnitPlatform() +} 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 new file mode 100644 index 0000000..789b82e --- /dev/null +++ b/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, 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 = + (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? = 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 = + (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 } + + /** + * Аналог 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, nlv: Int): List { + 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() + 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 } + } +} diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculator.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculator.kt new file mode 100644 index 0000000..f1e1714 --- /dev/null +++ b/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 +} diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorFactory.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorFactory.kt new file mode 100644 index 0000000..f7db8bc --- /dev/null +++ b/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) + } +} 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 new file mode 100644 index 0000000..53ea90f --- /dev/null +++ b/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 = 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 = 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 = List(7) { Vector3D() }, + val sdi: List = List(7) { 0.0 }, +) + +/** Полный результат расчета режима углового движения. */ +data class AngularMotionResult( + val mode: AngularMotionMode, + val startTime: Double, + val points: List, + val tauPoints: List = emptyList(), + val oeps: List = emptyList(), +) + +class AngularMotionCalculationException(message: String, cause: Throwable? = null) : RuntimeException(message, cause) 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 new file mode 100644 index 0000000..a287033 --- /dev/null +++ b/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 { + 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() + 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 { + 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 new file mode 100644 index 0000000..e5432fd --- /dev/null +++ b/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 { + val result = mutableListOf() + 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 + } +} 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 new file mode 100644 index 0000000..9a235e3 --- /dev/null +++ b/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, ys: List): 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)) 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 new file mode 100644 index 0000000..405516a --- /dev/null +++ b/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, + ) + } +} 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 new file mode 100644 index 0000000..09e7ad3 --- /dev/null +++ b/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)), + ) + } + } +} diff --git a/settings.gradle.kts b/settings.gradle.kts index db309b8..62e8eac 100644 --- a/settings.gradle.kts +++ b/settings.gradle.kts @@ -7,6 +7,7 @@ include(":config-repo") include(":libs:pcp-types-lib") include(":libs:ballistics-lib") +include(":libs:angular-motion-lib") include(":services:tle-monitoring-service") include(":services:slots-service") From b62a7cf8ef5a2da1ea74c1270d915ec34f762253 Mon Sep 17 00:00:00 2001 From: emelianov Date: Thu, 4 Jun 2026 13:22:48 +0300 Subject: [PATCH 3/3] =?UTF-8?q?=D0=9F=D0=A3=D0=A3=D0=94?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../nstart/pcp/angularmotion/AzimuthPUUD.kt | 4 +- .../nstart/pcp/angularmotion/InternalMath.kt | 20 ++++-- .../AngularMotionCalculatorSmokeTest.kt | 69 +++++++++++++++++++ 3 files changed, 87 insertions(+), 6 deletions(-) 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 a287033..80f870c 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 @@ -184,7 +184,9 @@ open class AzimuthPUUD( 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 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() 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 9a235e3..17a275f 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 @@ -107,11 +107,21 @@ internal fun rotationY(angle: Double): Matrix3D = Matrix3D().also { it.makeOyMat * которая уже используется в 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 +// val c = Matrix3D() +// c.second = rAbs.normSafe() +// c.third = (vAbs.rem(rAbs)).normSafe() +// c.first = c.second.rem(c.third).normSafe() + val c = rAbs.rem(vAbs) + val dummy = c.rem(rAbs) + val modC = c.module() + val modAbsRKA = rAbs.module() + val m = Matrix3D( + Vector3D( dummy.x/modC/modAbsRKA,c.x/modC, rAbs.x/modAbsRKA), + Vector3D( dummy.y/modC/modAbsRKA,c.y/modC, rAbs.y/modAbsRKA), + Vector3D( dummy.z/modC/modAbsRKA,c.z/modC, rAbs.z/modAbsRKA), + + ) + return m.transpose() } /** Матрица из OrbitalMotion::TLowOrbit::OrbBookToOrb. */ 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 09e7ad3..e660b93 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 @@ -1,8 +1,16 @@ package space.nstart.pcp.angularmotion +import ballistics.Ballistics import ballistics.orbitalPoints.timeStepper.AbstractStepper +import ballistics.types.BallisticsError +import ballistics.types.EarthType +import ballistics.types.InitialConditions import ballistics.types.OrbitalPoint +import ballistics.utils.fromDateTime import ballistics.utils.math.Vector3D +import ballistics.utils.toDateTime +import java.time.LocalDateTime +import kotlin.math.PI import kotlin.math.cos import kotlin.math.sin import kotlin.test.Test @@ -71,4 +79,65 @@ class AngularMotionCalculatorSmokeTest { ) } } + + + + @Test + fun chek(){ + 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), + nlv = 5, + 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 = -15.567 * PI / 180, + uprAngle = 0.0, + pointInCenter = false + ) + val rc = calculator.calculate(id) + + println(rc.mode) + println(rc.points.size) + println(rc.startTime) + + for (v in rc.points) { + println("${toDateTime( v.t)} ${v.orientation.tang * 180 / PI} ${v.orientation.kren * 180 / PI } ${v.orientation.risk * 180 / PI}") + } + } }