quadratic-dream/carto-revive
15
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added reverse callculation code. Unsure about quality yet.

Browse Source
This commit is contained in:
Yuki Kitagawa 2024-11-09 18:08:58 +08:00
parent 7aa59ff795
commit a69b4e54a2
2 changed files with 80 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/Craft/Krakensbane.cs
View File

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

91
src/Orbital/KeplerianElements.cs
View File

@ -44,10 +44,10 @@ public sealed class KeplerianElements(
/// </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 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(
/// <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;
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)