Skip to content

Commit

Permalink
ChangeOrbitalElements now uses Forward AutoDiff via Dual numbers/vectors
Browse files Browse the repository at this point in the history 
Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
  • Loading branch information
@lamont-granquist
lamont-granquist committed Aug 22, 2023
1 parent be01ad9 commit 89196a3
Show file tree
Hide file tree
Showing 6 changed files with 486 additions and 265 deletions.
482 changes: 242 additions & 240 deletions MechJeb2/MechJeb2.csproj
View file

Large diffs are not rendered by default.

43 changes: 34 additions & 9 deletions MechJeb2/MechJebLib/Core/Maths.cs
View file
Expand Up @@ -78,14 +78,10 @@ public static (double vT, double gammaT) ConvertApsidesTargetToFPA(double peR, d
public static double CircularVelocity(double mu, double r) => Sqrt(mu / r);

public static double PeriapsisFromKeplerian(double sma, double ecc) => sma * (1 - ecc);
public static Dual PeriapsisFromKeplerian(Dual sma, Dual ecc) => sma * (1 - ecc);

public static double ApoapsisFromKeplerian(double sma, double ecc, bool hyperbolic = true)
{
double apo = sma * (1 + ecc);
if (apo >= 0)
return apo;
return hyperbolic ? apo : double.MaxValue;
}
public static double ApoapsisFromKeplerian(double sma, double ecc) => sma * (1 + ecc);
public static Dual ApoapsisFromKeplerian(Dual sma, Dual ecc) => sma * (1 + ecc);

public static double PeriapsisFromStateVectors(double mu, V3 r, V3 v)
{
Expand All @@ -94,11 +90,25 @@ public static double PeriapsisFromStateVectors(double mu, V3 r, V3 v)
return PeriapsisFromKeplerian(sma, ecc);
}

public static double ApoapsisFromStateVectors(double mu, V3 r, V3 v, bool hyperbolic = true)
public static Dual PeriapsisFromStateVectors(Dual mu, DualV3 r, DualV3 v)
{
Dual sma, ecc;
(sma, ecc) = SmaEccFromStateVectors(mu, r, v);
return PeriapsisFromKeplerian(sma, ecc);
}

public static double ApoapsisFromStateVectors(double mu, V3 r, V3 v)
{
double sma, ecc;
(sma, ecc) = SmaEccFromStateVectors(mu, r, v);
return ApoapsisFromKeplerian(sma, ecc, hyperbolic);
return ApoapsisFromKeplerian(sma, ecc);
}

public static Dual ApoapsisFromStateVectors(Dual mu, DualV3 r, DualV3 v)
{
Dual sma, ecc;
(sma, ecc) = SmaEccFromStateVectors(mu, r, v);
return ApoapsisFromKeplerian(sma, ecc);
}

public static double EccFromStateVectors(double mu, V3 r, V3 v)
Expand All @@ -107,15 +117,30 @@ public static double EccFromStateVectors(double mu, V3 r, V3 v)
return eccvec.magnitude;
}

public static Dual EccFromStateVectors(Dual mu, DualV3 r, DualV3 v)
{
DualV3 eccvec = DualV3.Cross(v / mu, DualV3.Cross(r, v)) - r.normalized;
return eccvec.magnitude;
}

public static (double sma, double ecc) SmaEccFromStateVectors(double mu, V3 r, V3 v)
{
var h = V3.Cross(r, v);
double sma = SmaFromStateVectors(mu, r, v);
return (sma, Sqrt(Max(1 - h.sqrMagnitude / (sma * mu), 0)));
}

public static (Dual sma, Dual ecc) SmaEccFromStateVectors(Dual mu, DualV3 r, DualV3 v)
{
var h = DualV3.Cross(r, v);
Dual sma = SmaFromStateVectors(mu, r, v);
return (sma, Dual.Sqrt(Dual.Max(1 - h.sqrMagnitude / (sma * mu), 0)));
}

public static double SmaFromStateVectors(double mu, V3 r, V3 v) => mu / (2.0 * mu / r.magnitude - V3.Dot(v, v));

public static Dual SmaFromStateVectors(Dual mu, DualV3 r, DualV3 v) => mu / (2.0 * mu / r.magnitude - DualV3.Dot(v, v));

public static double IncFromStateVectors(V3 r, V3 v)
{
V3 hhat = V3.Cross(r, v).normalized;
Expand Down
89 changes: 77 additions & 12 deletions MechJeb2/MechJebLib/Maneuvers/ChangeOrbitalElement.cs
View file
Expand Up @@ -25,6 +25,57 @@ private struct Args
public Type Type;
}

private static void NLPFunction2(double[] x, double[] fi, double[,] jac, object obj)
{
var dv0 = new DualV3(x[0], x[1], 0, 1, 0, 0);
var dv1 = new DualV3(x[0], x[1], 0, 0, 1, 0);

var args = (Args)obj;
V3 p = args.P;
V3 q = args.Q;
double value = args.Value;
Type type = args.Type;

Dual sqM0 = dv0.sqrMagnitude;
Dual sqM1 = dv1.sqrMagnitude;

fi[0] = sqM0.M;
jac[0, 0] = sqM0.D;
jac[0, 1] = sqM1.D;

switch (type)
{
case Type.PERIAPSIS:
Dual peR0 = Maths.PeriapsisFromStateVectors(1.0, p, q + dv0) - value;
Dual peR1 = Maths.PeriapsisFromStateVectors(1.0, p, q + dv1) - value;
fi[1] = peR0.M;
jac[1, 0] = peR0.D;
jac[1, 1] = peR1.D;
break;
case Type.APOAPSIS:
Dual apR0 = 1.0 / Maths.ApoapsisFromStateVectors(1.0, p, q + dv0) - 1.0 / value;
Dual apR1 = 1.0 / Maths.ApoapsisFromStateVectors(1.0, p, q + dv1) - 1.0 / value;
fi[1] = apR0.M;
jac[1, 0] = apR0.D;
jac[1, 1] = apR1.D;
break;
case Type.SMA:
Dual sma0 = 1.0 / Maths.SmaFromStateVectors(1.0, p, q + dv0) - 1.0 / value;
Dual sma1 = 1.0 / Maths.SmaFromStateVectors(1.0, p, q + dv1) - 1.0 / value;
fi[1] = sma0.M;
jac[1, 0] = sma0.D;
jac[1, 1] = sma1.D;
break;
case Type.ECC:
Dual ecc0 = Maths.EccFromStateVectors(1.0, p, q + dv0) - value;
Dual ecc1 = Maths.EccFromStateVectors(1.0, p, q + dv1) - value;
fi[1] = ecc0.M;
jac[1, 0] = ecc0.D;
jac[1, 1] = ecc1.D;
break;
}
}

private static void NLPFunction(double[] x, double[] fi, object obj)
{
var dv = new V3(x[0], x[1], 0);
Expand All @@ -36,6 +87,7 @@ private static void NLPFunction(double[] x, double[] fi, object obj)
Type type = args.Type;

fi[0] = dv.sqrMagnitude;

switch (type)
{
case Type.PERIAPSIS:
Expand All @@ -54,7 +106,7 @@ private static void NLPFunction(double[] x, double[] fi, object obj)
}
}

private static V3 DeltaV(double mu, V3 r, V3 v, double value, Type type)
private static V3 DeltaV(double mu, V3 r, V3 v, double value, Type type, bool optguard=false)
{
if (!mu.IsFinite())
throw new ArgumentException("bad mu in ChangeOrbitalElement");
Expand Down Expand Up @@ -97,52 +149,65 @@ private static V3 DeltaV(double mu, V3 r, V3 v, double value, Type type)

var args = new Args { P = p, Q = q, Value = type == Type.ECC ? value : value / scale.LengthScale, Type = type };

alglib.minnlccreatef(NVARIABLES, x, DIFFSTEP, out alglib.minnlcstate state);
alglib.minnlccreate(NVARIABLES, x, out alglib.minnlcstate state);
alglib.minnlcsetalgosqp(state);
alglib.minnlcsetcond(state, EPSX, MAXITS);
alglib.minnlcsetnlc(state, NEQUALITYCONSTRAINTS, NINEQUALITYCONSTRAINTS);

alglib.minnlcoptimize(state, NLPFunction, null, args);
if (optguard)
{
alglib.minnlcoptguardsmoothness(state);
alglib.minnlcoptguardgradient(state, DIFFSTEP);
}

alglib.minnlcoptimize(state, NLPFunction2, null, args);
alglib.minnlcresults(state, out x, out alglib.minnlcreport rep);

if (rep.terminationtype < 0)
throw new Exception(
$"DeltaVToChangeApsis({mu}, {r}, {v}, {value}, {type}): SQP solver terminated abnormally: {rep.terminationtype}"
$"ChangeOrbitalElement.DeltaV({mu}, {r}, {v}, {value}, {type}): SQP solver terminated abnormally: {rep.terminationtype}"
);

if (optguard)
{
alglib.minnlcoptguardresults(state, out alglib.optguardreport ogrep);
if (ogrep.badgradsuspected || ogrep.nonc0suspected || ogrep.nonc1suspected)
throw new Exception("alglib optguard caught an error, i should report better on errors now");
}

return rot * new V3(x[0], x[1], 0) * scale.VelocityScale;
}

public static V3 ChangePeriapsis(double mu, V3 r, V3 v, double peR)
public static V3 ChangePeriapsis(double mu, V3 r, V3 v, double peR, bool optguard = false)
{
if (peR < 0 || (peR > r.magnitude) | !peR.IsFinite())
throw new ArgumentException($"Bad periapsis in ChangeOrbitalElement = {peR}");

return DeltaV(mu, r, v, peR, Type.PERIAPSIS);
return DeltaV(mu, r, v, peR, Type.PERIAPSIS, optguard);
}

public static V3 ChangeApoapsis(double mu, V3 r, V3 v, double apR)
public static V3 ChangeApoapsis(double mu, V3 r, V3 v, double apR, bool optguard = false)
{
if (!apR.IsFinite() || (apR > 0 && apR < r.magnitude))
throw new ArgumentException($"Bad apoapsis in ChangeOrbitalElement = {apR}");

return DeltaV(mu, r, v, apR, Type.APOAPSIS);
return DeltaV(mu, r, v, apR, Type.APOAPSIS, optguard);
}

public static V3 ChangeSMA(double mu, V3 r, V3 v, double sma)
public static V3 ChangeSMA(double mu, V3 r, V3 v, double sma, bool optguard = false)
{
if (!sma.IsFinite())
throw new ArgumentException($"Bad SMA in ChangeOrbitalElement = {sma}");

return DeltaV(mu, r, v, sma, Type.SMA);
return DeltaV(mu, r, v, sma, Type.SMA, optguard);
}

public static V3 ChangeECC(double mu, V3 r, V3 v, double ecc)
public static V3 ChangeECC(double mu, V3 r, V3 v, double ecc, bool optguard = false)
{
if (!ecc.IsFinite() || ecc < 0)
throw new ArgumentException($"Bad Ecc in ChangeOrbitalElement = {ecc}");

return DeltaV(mu, r, v, ecc, Type.ECC);
return DeltaV(mu, r, v, ecc, Type.ECC, optguard);
}
}
}
67 changes: 67 additions & 0 deletions MechJeb2/MechJebLib/Primitives/Dual.cs
View file
@@ -0,0 +1,67 @@
/*
* Copyright Lamont Granquist, Sebastien Gaggini and the MechJeb contributors
* SPDX-License-Identifier: LicenseRef-PD-hp OR Unlicense OR CC0-1.0 OR 0BSD OR MIT-0 OR MIT OR LGPL-2.1+
*/

#nullable enable

using System;
using System.Globalization;
using MechJebLib.Utils;

namespace MechJebLib.Primitives
{
public class Dual : IEquatable<Dual>, IComparable<Dual>, IFormattable
{
public readonly double M;
public readonly double D;

public Dual(double m, double d = 0)
{
M = m;
D = d;
}

public static Dual Sin(Dual d) => new Dual(Math.Sin(d.M), Math.Cos(d.M) * d.D);
public static Dual Cos(Dual d) => new Dual(Math.Cos(d.M), -Math.Sin(d.M) * d.D);

public static Dual Tan(Dual d)
{
double t = Math.Tan(d.M);
return new Dual(t, (1.0 + t * t) * d.D);
}

public static Dual Log(Dual d) => new Dual(Math.Log(d.M), 1 / d.M * d.D);
public static Dual Powi(Dual d, int k) => new Dual(Statics.Powi(d.M, k), k * Statics.Powi(d.M, k - 1) * d.D);
public static Dual Abs(Dual d) => new Dual(Math.Abs(d.M), (d.M < 0 ? -1 : 1) * d.D);
public static Dual Exp(Dual d) => new Dual(Math.Exp(d.M), Math.Exp(d.M) * d.D);
public static Dual Sqrt(Dual d) => new Dual(Math.Sqrt(d.M), 1.0 / (2 * Math.Sqrt(d.M)) * d.D);
public static Dual Max(Dual d, int i) => new Dual(Math.Max(d.M, i), d.M > i ? d.D : 0);

public static Dual operator -(Dual x) => new Dual(-x.M, -x.D);
public static Dual operator +(Dual lhs, Dual rhs) => new Dual(lhs.M + rhs.M, lhs.D + rhs.D);
public static Dual operator -(Dual lhs, Dual rhs) => new Dual(lhs.M - rhs.M, lhs.D - rhs.D);
public static Dual operator *(Dual lhs, Dual rhs) => new Dual(lhs.M * rhs.M, lhs.D * rhs.M + rhs.D * lhs.M);
public static Dual operator /(Dual lhs, Dual rhs) => new Dual(lhs.M / rhs.M, (lhs.D * rhs.M - lhs.M * rhs.D) / (rhs.M * rhs.M));

public static explicit operator double(Dual d) => d.M;
public static implicit operator Dual(double d) => new Dual(d);

// ReSharper disable once CompareOfFloatsByEqualityOperator
public bool Equals(Dual other) => M == other.M && D.Equals(other.D);

public int CompareTo(Dual other) => M.CompareTo(other.M);

public override string ToString() => ToString(null, CultureInfo.InvariantCulture.NumberFormat);

public string ToString(string? format) => ToString(format, CultureInfo.InvariantCulture.NumberFormat);

public string ToString(string? format, IFormatProvider formatProvider)
{
if (string.IsNullOrEmpty(format))
format = "G";
return
$"{M.ToString(format, formatProvider)} + {D.ToString(format, formatProvider)}ϵ";
}
}
}
62 changes: 62 additions & 0 deletions MechJeb2/MechJebLib/Primitives/DualV3.cs
View file
@@ -0,0 +1,62 @@
/*
* Copyright Lamont Granquist, Sebastien Gaggini and the MechJeb contributors
* SPDX-License-Identifier: LicenseRef-PD-hp OR Unlicense OR CC0-1.0 OR 0BSD OR MIT-0 OR MIT OR LGPL-2.1+
*/

#nullable enable

using System;

namespace MechJebLib.Primitives
{
public class DualV3 : IEquatable<DualV3>, IComparable<DualV3>, IFormattable
{
public readonly V3 M;
public readonly V3 D;

public Dual x => new Dual(M.x, D.x);
public Dual y => new Dual(M.y, D.y);
public Dual z => new Dual(M.z, D.z);

private DualV3(V3 m, V3? dx = null)
{
M = m;
D = dx ?? V3.zero;
}

public DualV3(Dual x, Dual y, Dual z) : this(x.M, y.M, z.M, x.D, y.D, z.D) { }
public DualV3(double mx, double my, double mz, double dx, double dy, double dz) : this(new V3(mx, my, mz), new V3(dx, dy, dz)) { }

public Dual sqrMagnitude => x * x + y * y + z * z;
public Dual magnitude => Dual.Sqrt(x * x + y * y + z * z);
public DualV3 normalized => this / magnitude;

public static DualV3 operator +(DualV3 a, DualV3 b) => new DualV3(a.x + b.x, a.y + b.y, a.z + b.z);

public static DualV3 operator -(DualV3 a, DualV3 b) => new DualV3(a.x - b.x, a.y - b.y, a.z - b.z);

public static DualV3 operator *(DualV3 a, DualV3 b) => new DualV3(a.x * b.x, a.y * b.y, a.z * b.z);

public static DualV3 operator /(DualV3 a, DualV3 b) => new DualV3(a.x / b.x, a.y / b.y, a.z / b.z);

public static DualV3 operator -(DualV3 a) => new DualV3(-a.x, -a.y, -a.z);

public static DualV3 operator *(DualV3 a, Dual d) => new DualV3(a.x * d, a.y * d, a.z * d);

public static DualV3 operator *(Dual d, DualV3 a) => new DualV3(a.x * d, a.y * d, a.z * d);

public static DualV3 operator /(DualV3 a, Dual d) => new DualV3(a.x / d, a.y / d, a.z / d);

public static explicit operator V3(DualV3 d) => d.M;
public static implicit operator DualV3(V3 d) => new DualV3(d);

public bool Equals(DualV3 other) => throw new NotImplementedException();
public int CompareTo(DualV3 other) => throw new NotImplementedException();
public string ToString(string format, IFormatProvider formatProvider) => throw new NotImplementedException();

public static DualV3 Cross(DualV3 v1, DualV3 v2) =>
new DualV3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);

public static Dual Dot(DualV3 v1, DualV3 v2) => v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
}
8 changes: 4 additions & 4 deletions MechJebLibTest/Maths/FunctionsTests.cs
View file
Expand Up @@ -806,7 +806,7 @@ private void ChangeOrbitalElementTest()
newR *= rscale;
double mu = rscale * vscale * vscale;

V3 dv = ChangeOrbitalElement.ChangePeriapsis(mu, r, v, newR);
V3 dv = ChangeOrbitalElement.ChangePeriapsis(mu, r, v, newR, true);
MechJebLib.Core.Maths.PeriapsisFromStateVectors(mu, r, v + dv).ShouldEqual(newR, 1e-9);

// validate this API works left handed.
Expand All @@ -815,12 +815,12 @@ private void ChangeOrbitalElementTest()

// now test apoapsis changing to elliptical
newR = random.NextDouble() * rscale * 1e9 + r.magnitude;
V3 dv3 = ChangeOrbitalElement.ChangeApoapsis(mu, r, v, newR);
V3 dv3 = ChangeOrbitalElement.ChangeApoapsis(mu, r, v, newR, true);
MechJebLib.Core.Maths.ApoapsisFromStateVectors(mu, r, v + dv3).ShouldEqual(newR, 1e-5);

// now test apoapsis changing to hyperbolic
newR = -(random.NextDouble() * 1e9 + 1e3) * rscale;
V3 dv4 = ChangeOrbitalElement.ChangeApoapsis(mu, r, v, newR);
V3 dv4 = ChangeOrbitalElement.ChangeApoapsis(mu, r, v, newR, true);
MechJebLib.Core.Maths.ApoapsisFromStateVectors(mu, r, v + dv4).ShouldEqual(newR, 1e-5);

// now test changing the SMA.
Expand All @@ -831,7 +831,7 @@ private void ChangeOrbitalElementTest()
v = new V3(23370.6359939819, 12558.3695536628, 17864.1347044172);
newR = 35354091.544774853;
*/
V3 dv5 = ChangeOrbitalElement.ChangeSMA(mu, r, v, newR);
V3 dv5 = ChangeOrbitalElement.ChangeSMA(mu, r, v, newR, true);
MechJebLib.Core.Maths.SmaFromStateVectors(mu, r, v + dv5).ShouldEqual(newR, 1e-9);

// now test changing the Ecc
Expand Down

0 comments on commit 89196a3

Please sign in to comment.