Added reverse callculation code. Unsure about quality yet.

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);
+            var (elements, epoch) = KeplerianElements.FromMotionStateFull(state, celestialBody.Mu);
-        }
-
-        ImGui.Begin("Krakensbane");
-        ImGui.Text($"Origin: {originPosition}");
-        ImGui.Text($"Focused vessel: {FocusedVessel?.Name}");
-        if (elements != null)
-        {
             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();
     }

src/Orbital/KeplerianElements.cs

@@ -44,10 +44,10 @@ public sealed class KeplerianElements(
     /// </summary>
     public double ta = ta;
 
-    public double IncDegree => inc * Math.PI / 180;
+    public double IncDegree => inc * 180 / Math.PI;
-    public double LanDegree => lan * Math.PI / 180;
+    public double LanDegree => lan * 180 / Math.PI;
-    public double ApeDegree => ape * Math.PI / 180;
+    public double ApeDegree => ape * 180 / Math.PI;
-    public double TaDegree => ta * Math.PI / 180;
+    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(
     /// <param name="mu">The standard gravitational parameter, mu = G(M+m)</param>
     /// <param name="epoch">The time of the query</param>
     /// <returns></returns>
+    // 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;
+        return FromMotionStateFull(state, mu).Item1;
-        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);
     }
 
     public static KeplerianElements EquatorialCircular(double sma, double ta)