From 7aa59ff795abd79b8e8aab13cd5b6d14ecc83cc8 Mon Sep 17 00:00:00 2001 From: Yuki Kitagawa Date: Sat, 9 Nov 2024 17:54:04 +0800 Subject: [PATCH] Add orbit calculation --- src/Craft/Krakensbane.cs | 25 ++++++- src/Orbital/CelestialBody.cs | 44 +++++++++++ src/Orbital/KeplerianElements.cs | 121 +++++++++++++++++++++++++++++++ src/Orbital/PhysicalConstants.cs | 27 +++++++ 4 files changed, 216 insertions(+), 1 deletion(-) create mode 100644 src/Orbital/CelestialBody.cs create mode 100644 src/Orbital/KeplerianElements.cs create mode 100644 src/Orbital/PhysicalConstants.cs diff --git a/src/Craft/Krakensbane.cs b/src/Craft/Krakensbane.cs index 9601b31..f74ab5a 100644 --- a/src/Craft/Krakensbane.cs +++ b/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; /// @@ -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(); } } diff --git a/src/Orbital/CelestialBody.cs b/src/Orbital/CelestialBody.cs new file mode 100644 index 0000000..7882a04 --- /dev/null +++ b/src/Orbital/CelestialBody.cs @@ -0,0 +1,44 @@ +namespace Quadratic.Carto.Orbital; + +/// +/// Represents the basic properties of a celestial body. +/// +/// The mass of the body, in kilograms +/// The radius of the body, in meters +/// The rotation period of the body, in seconds +public sealed record CelestialBody( + double Mass, + double Radius, + double RotationPeriod +) +{ + /// + /// The gravitational parameter of the body, mu = G(M+m), in cubic meters per second squared. + /// + public double Mu => PhysicalConstants.GravitationalConstant * Mass; + + /// + /// The surface gravity of the body, in meters per second squared. + /// + public double SurfaceGravity => Mu / (Radius * Radius); + + /// + /// Get the celestial body from its surface gravity, radius, and rotation period. + /// + /// + /// + /// + /// + 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})"; + } +} diff --git a/src/Orbital/KeplerianElements.cs b/src/Orbital/KeplerianElements.cs new file mode 100644 index 0000000..e943b36 --- /dev/null +++ b/src/Orbital/KeplerianElements.cs @@ -0,0 +1,121 @@ +using System; +using Vim.Math3d; + +namespace Quadratic.Carto.Orbital; + +/// +/// A list of keplerian orbital elements, representing the orbit around a celestial body. +/// +/// +/// https://orbital-mechanics.space/classical-orbital-elements/orbital-elements-and-the-state-vector.html +/// +public sealed class KeplerianElements( + double ecc, double sma, double inc, + double lan, double ape, double ta +) +{ + /// + /// Eccentricity of the orbit. + /// + public double ecc = ecc; + + /// + /// Semi-major axis of the orbit, in meters. + /// + public double sma = sma; + + /// + /// Inclination of the orbit, in radians [0, π). + /// + public double inc = inc; + + /// + /// Longitude of the ascending node, in radians [0, 2π). + /// + public double lan = lan; + + /// + /// Argument of periapsis, in radians [0, 2π). + /// + public double ape = ape; + + /// + /// True anomaly, in radians [0, 2π). + /// + 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})"; + } + + /// + /// Returns the position and velocity of the body at the given time. + /// + /// The standard gravitational parameter, mu = G(M+m) + /// The time of the query + /// + 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; +} diff --git a/src/Orbital/PhysicalConstants.cs b/src/Orbital/PhysicalConstants.cs new file mode 100644 index 0000000..bf125ca --- /dev/null +++ b/src/Orbital/PhysicalConstants.cs @@ -0,0 +1,27 @@ +namespace Quadratic.Carto.Orbital; + +/// +/// Provides commonly used physical constants. +/// +public static class PhysicalConstants +{ + /// + /// The speed of light in a vacuum, in meters per second. + /// + public const double LightSpeed = 299_792_458.0; + + /// + /// The gravitational constant, in cubic meters per kilogram per second squared. + /// + public const double GravitationalConstant = 6.67430e-11; + + /// + /// Alias for the speed of light in a vacuum, in meters per second. + /// + public const double c = LightSpeed; + + /// + /// Alias for the gravitational constant, in cubic meters per kilogram per second squared. + /// + public const double G = GravitationalConstant; +}