Add orbit calculation

src/Craft/Krakensbane.cs

@@ -2,6 +2,7 @@ using System.Diagnostics;
 using Godot;
 using ImGuiNET;
 using Quadratic.Carto.MathExt;
+using Quadratic.Carto.Orbital;
 using Vim.Math3d;
 
 namespace Quadratic.Carto.Craft;
@@ -23,6 +24,8 @@ public sealed partial class Krakensbane : Node3D
     const float maxDistance = 500.0f;
     const float planetRadius = 6372.0f;
 
+    CelestialBody celestialBody = CelestialBody.FromSurfaceGravity(9.81, planetRadius, 86164.1);
+
     DVector3 originPosition = DVector3.UnitY * planetRadius;
 
     /// <summary>
@@ -80,7 +83,7 @@ public sealed partial class Krakensbane : Node3D
                 var gravityVector = -globalPosition.Normalize();
                 var floatGravityVector = gravityVector.AsSingle().AsGodot();
                 var distance = globalPosition.Length();
-                var gravity = 9.81f * planetRadius * planetRadius / (distance * distance);
+                var gravity = celestialBody.Mu / (distance * distance);
                 rb.ApplyCentralForce(floatGravityVector * (float)gravity);
             }
         }
@@ -94,8 +97,28 @@ public sealed partial class Krakensbane : Node3D
 
     public override void _Process(double delta)
     {
+        // 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)
+        {
+            ImGui.Text("Orbit");
+            ImGui.BeginGroup();
+            ImGui.Text($"Ap: {elements.Apoapsis}");
+            ImGui.Text($"Pe: {elements.Periapsis}");
+            ImGui.Text($"Period: {elements.Period(celestialBody.Mu)}");
+            ImGui.EndGroup();
+        }
         ImGui.End();
     }
 }