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}") + } + } }