From a69b4e54a2a6cc68dd691deb5113a5c369dccf43 Mon Sep 17 00:00:00 2001 From: Yuki Kitagawa Date: Sat, 9 Nov 2024 18:08:58 +0800 Subject: [PATCH] Added reverse callculation code. Unsure about quality yet. --- src/Craft/Krakensbane.cs | 31 +++++++---- src/Orbital/KeplerianElements.cs | 91 +++++++++++++++++++++----------- 2 files changed, 80 insertions(+), 42 deletions(-) diff --git a/src/Craft/Krakensbane.cs b/src/Craft/Krakensbane.cs index f74ab5a..16f73b6 100644 --- a/src/Craft/Krakensbane.cs +++ b/src/Craft/Krakensbane.cs @@ -97,27 +97,36 @@ public sealed partial class Krakensbane : Node3D public override void _Process(double delta) { + + ImGui.Begin("Krakensbane"); + ImGui.Text($"Origin: {originPosition}"); + ImGui.Text($"Focused vessel: {FocusedVessel?.Name}"); + // Debug the orbit of the focused vessel - KeplerianElements? elements = null; if (FocusedVessel != null && FocusedVessel is RigidBody3D rigidBody3D) { var position = GetPositionOf(rigidBody3D); var velocity = rigidBody3D.LinearVelocity.AsVim().AsDouble(); var state = new DStateVector(position, velocity); - elements = KeplerianElements.FromMotionState(state, celestialBody.Mu); - } - - ImGui.Begin("Krakensbane"); - ImGui.Text($"Origin: {originPosition}"); - ImGui.Text($"Focused vessel: {FocusedVessel?.Name}"); - if (elements != null) - { + var (elements, epoch) = KeplerianElements.FromMotionStateFull(state, celestialBody.Mu); ImGui.Text("Orbit"); - ImGui.BeginGroup(); + ImGui.Indent(); ImGui.Text($"Ap: {elements.Apoapsis}"); ImGui.Text($"Pe: {elements.Periapsis}"); ImGui.Text($"Period: {elements.Period(celestialBody.Mu)}"); - ImGui.EndGroup(); + ImGui.Text($"Inclination: {elements.IncDegree}deg"); + ImGui.Text($"Epoch: {epoch}"); + ImGui.Unindent(); + + // Roundtrip test + var newState = elements.MotionStateAt(celestialBody.Mu, epoch); + ImGui.Text("Roundtrip test"); + ImGui.Indent(); + ImGui.Text($"Original position: {position:F6}"); + ImGui.Text($"New position: {newState.Position:F6}"); + ImGui.Text($"Original velocity: {velocity:F6}"); + ImGui.Text($"New velocity: {newState.Velocity:F6}"); + ImGui.Unindent(); } ImGui.End(); } diff --git a/src/Orbital/KeplerianElements.cs b/src/Orbital/KeplerianElements.cs index e943b36..772ed14 100644 --- a/src/Orbital/KeplerianElements.cs +++ b/src/Orbital/KeplerianElements.cs @@ -44,10 +44,10 @@ public sealed class KeplerianElements( /// public double ta = ta; - public double IncDegree => inc * Math.PI / 180; - public double LanDegree => lan * Math.PI / 180; - public double ApeDegree => ape * Math.PI / 180; - public double TaDegree => ta * Math.PI / 180; + public double IncDegree => inc * 180 / Math.PI; + public double LanDegree => lan * 180 / Math.PI; + public double ApeDegree => ape * 180 / Math.PI; + public double TaDegree => ta * 180 / Math.PI; public double Apoapsis => sma * (1 + ecc); public double Periapsis => sma * (1 - ecc); @@ -68,39 +68,68 @@ public sealed class KeplerianElements( /// The standard gravitational parameter, mu = G(M+m) /// The time of the query /// + // TODO: automatically translated, not verified public DStateVector MotionStateAt(double mu, double epoch) { - throw new NotImplementedException(); + double n = Math.Sqrt(mu / (sma * sma * sma)); + double M = n * (epoch - 0); // Assuming epoch is time since periapsis + + double EA = ta; // Assuming true anomaly is given as eccentric anomaly + double MA = EA - ecc * Math.Sin(EA); + + double nu = 2 * Math.Atan(Math.Sqrt((1 + ecc) / (1 - ecc)) * Math.Tan(EA / 2)); + double r = sma * (1 - ecc * Math.Cos(EA)); + double h = Math.Sqrt(mu * sma * (1 - ecc * ecc)); + + double Om = lan; + double w = ape; + + double X = r * (Math.Cos(Om) * Math.Cos(w + nu) - Math.Sin(Om) * Math.Sin(w + nu) * Math.Cos(inc)); + double Z = r * (Math.Sin(Om) * Math.Cos(w + nu) + Math.Cos(Om) * Math.Sin(w + nu) * Math.Cos(inc)); // Swapped Y and Z + double Y = r * (Math.Sin(inc) * Math.Sin(w + nu)); // Swapped Y and Z + + double p = sma * (1 - ecc * ecc); + + double V_X = (X * h * ecc / (r * p)) * Math.Sin(nu) - (h / r) * (Math.Cos(Om) * Math.Sin(w + nu) + Math.Sin(Om) * Math.Cos(w + nu) * Math.Cos(inc)); + double V_Z = (Z * h * ecc / (r * p)) * Math.Sin(nu) - (h / r) * (Math.Sin(Om) * Math.Sin(w + nu) - Math.Cos(Om) * Math.Cos(w + nu) * Math.Cos(inc)); // Swapped V_Y and V_Z + double V_Y = (Y * h * ecc / (r * p)) * Math.Sin(nu) + (h / r) * (Math.Cos(w + nu) * Math.Sin(inc)); // Swapped V_Y and V_Z + + return new DStateVector(new DVector3(X, Y, Z), new DVector3(V_X, V_Y, V_Z)); + } + + public static (KeplerianElements, double) FromMotionStateFull(DStateVector state, double mu) + { + var r_vec = state.Position; + var v_vec = state.Velocity; + + var h_vec = r_vec.Cross(v_vec); + var h = h_vec.Length(); + + var r = r_vec.Length(); + var v = v_vec.Length(); + + var E = 0.5 * (v * v) - mu / r; + var a = -mu / (2 * E); + var e = Math.Sqrt(1 - (h * h) / (a * mu)); + var i = Math.Acos(h_vec.Z / h); + var omega_LAN = Math.Atan2(h_vec.X, -h_vec.Y); + + var lat = Math.Atan2(r_vec.Z / Math.Sin(i), r_vec.X * Math.Cos(omega_LAN) + r_vec.Y * Math.Sin(omega_LAN)); + var p = a * (1 - e * e); + var nu = Math.Atan2(Math.Sqrt(p / mu) * DVector3.Dot(r_vec, v_vec), p - r); + var omega_AP = lat - nu; + + var EA = 2 * Math.Atan(Math.Sqrt((1 - e) / (1 + e)) * Math.Tan(nu / 2)); + var n = Math.Sqrt(mu / (a * a * a)); + var T = -1 / n * (EA - e * Math.Sin(EA)); + + var keplerianElements = new KeplerianElements(e, a, i, omega_LAN, omega_AP, nu); + return (keplerianElements, T); } public static KeplerianElements FromMotionState(DStateVector state, double mu) { - DVector3 position = state.Position; - DVector3 velocity = state.Velocity; - - double radius = position.Length(); - double speed = velocity.Length(); - double vRadial = DVector3.Dot(position, velocity) / radius; - double vTangential = Math.Sqrt(speed * speed - vRadial * vRadial); - - DVector3 angularMomentum = position.Cross(velocity); - double h = angularMomentum.Length(); - - double inclination = Math.Acos(angularMomentum.Z / h); - - DVector3 kHat = new DVector3(0, 0, 1); - DVector3 n = kHat.Cross(angularMomentum); - double nNorm = n.Length(); - double lan = Math.Acos(n.X / nNorm); - - DVector3 eVec = velocity.Cross(angularMomentum) / mu - position / radius; - double ecc = eVec.Length(); - - double a = 1 / (2 / radius - speed * speed / mu); - - double ta = Math.Acos((a * (1 - ecc * ecc) / radius - 1) / ecc); - - return new KeplerianElements(ecc, a, inclination, lan, 0, ta); + return FromMotionStateFull(state, mu).Item1; } public static KeplerianElements EquatorialCircular(double sma, double ta)