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();
     }
 }

src/Orbital/CelestialBody.cs

@@ -0,0 +1,44 @@
+namespace Quadratic.Carto.Orbital;
+
+/// <summary>
+/// Represents the basic properties of a celestial body.
+/// </summary>
+/// <param name="Mass">The mass of the body, in kilograms</param>
+/// <param name="Radius">The radius of the body, in meters</param>
+/// <param name="RotationPeriod">The rotation period of the body, in seconds</param>
+public sealed record CelestialBody(
+    double Mass,
+    double Radius,
+    double RotationPeriod
+)
+{
+    /// <summary>
+    /// The gravitational parameter of the body, mu = G(M+m), in cubic meters per second squared.
+    /// </summary>
+    public double Mu => PhysicalConstants.GravitationalConstant * Mass;
+
+    /// <summary>
+    /// The surface gravity of the body, in meters per second squared.
+    /// </summary>
+    public double SurfaceGravity => Mu / (Radius * Radius);
+
+    /// <summary>
+    /// Get the celestial body from its surface gravity, radius, and rotation period.
+    /// </summary>
+    /// <param name="surfaceGravity"></param>
+    /// <param name="radius"></param>
+    /// <param name="rotationPeriod"></param>
+    /// <returns></returns>
+    public static CelestialBody FromSurfaceGravity(double surfaceGravity, double radius, double rotationPeriod)
+    {
+        var mass = surfaceGravity * radius * radius / PhysicalConstants.GravitationalConstant;
+        return new CelestialBody(mass, radius, rotationPeriod);
+    }
+
+    public static CelestialBody Earth = new CelestialBody(5.972e24, 6371e3, 86164.1);
+
+    public override string ToString()
+    {
+        return $"CelestialBody(Mass: {Mass}, Radius: {Radius}, RotationPeriod: {RotationPeriod})";
+    }
+}

src/Orbital/KeplerianElements.cs

@@ -0,0 +1,121 @@
+using System;
+using Vim.Math3d;
+
+namespace Quadratic.Carto.Orbital;
+
+/// <summary>
+/// A list of keplerian orbital elements, representing the orbit around a celestial body.
+/// </summary>
+/// <remarks>
+/// https://orbital-mechanics.space/classical-orbital-elements/orbital-elements-and-the-state-vector.html
+/// </remarks>
+public sealed class KeplerianElements(
+    double ecc, double sma, double inc,
+    double lan, double ape, double ta
+)
+{
+    /// <summary>
+    /// Eccentricity of the orbit.
+    /// </summary>
+    public double ecc = ecc;
+
+    /// <summary>
+    /// Semi-major axis of the orbit, in meters.
+    /// </summary>
+    public double sma = sma;
+
+    /// <summary>
+    /// Inclination of the orbit, in radians [0, π).
+    /// </summary>
+    public double inc = inc;
+
+    /// <summary>
+    /// Longitude of the ascending node, in radians [0, 2π).
+    /// </summary>
+    public double lan = lan;
+
+    /// <summary>
+    /// Argument of periapsis, in radians [0, 2π).
+    /// </summary>
+    public double ape = ape;
+
+    /// <summary>
+    /// True anomaly, in radians [0, 2π).
+    /// </summary>
+    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 Apoapsis => sma * (1 + ecc);
+    public double Periapsis => sma * (1 - ecc);
+
+    public double Period(double mu)
+    {
+        return 2 * Math.PI * Math.Sqrt(sma * sma * sma / mu);
+    }
+
+    public override string ToString()
+    {
+        return $"KeplerianElements(ecc: {ecc:F6}, sma: {sma:F6}, inc: {inc:F6}, lan: {lan:F6}, ape: {ape:F6}, ta: {ta:F6})";
+    }
+
+    /// <summary>
+    /// Returns the position and velocity of the body at the given time.
+    /// </summary>
+    /// <param name="mu">The standard gravitational parameter, mu = G(M+m)</param>
+    /// <param name="epoch">The time of the query</param>
+    /// <returns></returns>
+    public DStateVector MotionStateAt(double mu, double epoch)
+    {
+        throw new NotImplementedException();
+    }
+
+    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);
+    }
+
+    public static KeplerianElements EquatorialCircular(double sma, double ta)
+    {
+        return new KeplerianElements(0, sma, 0, 0, 0, ta);
+    }
+
+    public static KeplerianElements Circular(double sma, double inc, double lan, double ta)
+    {
+        return new KeplerianElements(0, sma, inc, lan, 0, ta);
+    }
+}
+
+public struct DStateVector(DVector3 position, DVector3 velocity)
+{
+    public DVector3 Position = position;
+    public DVector3 Velocity = velocity;
+}

src/Orbital/PhysicalConstants.cs

@@ -0,0 +1,27 @@
+namespace Quadratic.Carto.Orbital;
+
+/// <summary>
+/// Provides commonly used physical constants.
+/// </summary>
+public static class PhysicalConstants
+{
+    /// <summary>
+    /// The speed of light in a vacuum, in meters per second.
+    /// </summary>
+    public const double LightSpeed = 299_792_458.0;
+
+    /// <summary>
+    /// The gravitational constant, in cubic meters per kilogram per second squared.
+    /// </summary>
+    public const double GravitationalConstant = 6.67430e-11;
+
+    /// <summary>
+    /// Alias for the speed of light in a vacuum, in meters per second.
+    /// </summary>
+    public const double c = LightSpeed;
+
+    /// <summary>
+    /// Alias for the gravitational constant, in cubic meters per kilogram per second squared.
+    /// </summary>
+    public const double G = GravitationalConstant;
+}