Skip to content

Commit

Permalink
ReturnFromMoon: change periapsis constraint
Browse files Browse the repository at this point in the history 
removing normalization makes it converge much faster.

slight impact on accuracy but cuts the time by over 50%.

also add an always-on-optguard test so i make sure not to screw up the
jacobians.

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
  • Loading branch information
@lamont-granquist
lamont-granquist committed Nov 20, 2023
1 parent 675f7a2 commit bf9e8dc
Show file tree
Hide file tree
Showing 2 changed files with 68 additions and 29 deletions.
57 changes: 31 additions & 26 deletions MechJebLib/Maneuvers/ReturnFromMoon.cs
View file
Expand Up @@ -231,14 +231,14 @@ private static void NLPFunction(double[] x, double[] fi, double[,] jac, object o
* periapsis condition
*/

Dual p = DualV3.Dot(new DualV3(rf, new V3(1, 0, 0)).normalized, vf.normalized);
Dual p = DualV3.Dot(new DualV3(rf, new V3(1, 0, 0)), vf);
fi[13] = p.M;
jac[13, 14] = p.D;
jac[13, 15] = DualV3.Dot(new DualV3(rf, new V3(0, 1, 0)).normalized, vf.normalized).D;
jac[13, 16] = DualV3.Dot(new DualV3(rf, new V3(0, 0, 1)).normalized, vf.normalized).D;
jac[13, 17] = DualV3.Dot(rf.normalized, new DualV3(vf, new V3(1, 0, 0)).normalized).D;
jac[13, 18] = DualV3.Dot(rf.normalized, new DualV3(vf, new V3(0, 1, 0)).normalized).D;
jac[13, 19] = DualV3.Dot(rf.normalized, new DualV3(vf, new V3(0, 0, 1)).normalized).D;
jac[13, 15] = DualV3.Dot(new DualV3(rf, new V3(0, 1, 0)), vf).D;
jac[13, 16] = DualV3.Dot(new DualV3(rf, new V3(0, 0, 1)), vf).D;
jac[13, 17] = DualV3.Dot(rf, new DualV3(vf, new V3(1, 0, 0))).D;
jac[13, 18] = DualV3.Dot(rf, new DualV3(vf, new V3(0, 1, 0))).D;
jac[13, 19] = DualV3.Dot(rf, new DualV3(vf, new V3(0, 0, 1))).D;

/*
* periapsis constraint
Expand Down Expand Up @@ -356,15 +356,16 @@ private static double[] GenerateInitialGuess(Args args)
return x;
}

//private const double DIFFSTEP = 1e-9;
private const double EPSX = 1e-3;
private const int MAXITS = 10000;
private const double DIFFSTEP = 1e-9;
private const double EPSX = 1e-3;
private const int MAXITS = 10000;

private const int NVARIABLES = 20;
private const int NEQUALITYCONSTRAINTS = 15;
private const int NINEQUALITYCONSTRAINTS = 0;

private static (V3 V, double dt) ManeuverScaled(Args args, double dtmin = double.NegativeInfinity, double dtmax = double.PositiveInfinity)
private static (V3 V, double dt) ManeuverScaled(Args args, double dtmin = double.NegativeInfinity, double dtmax = double.PositiveInfinity,
bool optguard = false)
{
Scale planetScale = args.PlanetScale;
V3 moonR0 = args.MoonR0;
Expand Down Expand Up @@ -399,10 +400,11 @@ private static (V3 V, double dt) ManeuverScaled(Args args, double dtmin = double
alglib.minnlcsetcond(state, EPSX, MAXITS);
alglib.minnlcsetnlc(state, NEQUALITYCONSTRAINTS, NINEQUALITYCONSTRAINTS);

/*
alglib.minnlcoptguardsmoothness(state);
alglib.minnlcoptguardgradient(state, DIFFSTEP);
*/
if (optguard)
{
alglib.minnlcoptguardsmoothness(state);
alglib.minnlcoptguardgradient(state, DIFFSTEP);
}

double[] fi = new double[NEQUALITYCONSTRAINTS + NINEQUALITYCONSTRAINTS + 1];
double[,] jac = new double[NEQUALITYCONSTRAINTS + NINEQUALITYCONSTRAINTS + 1, NVARIABLES];
Expand All @@ -415,16 +417,17 @@ private static (V3 V, double dt) ManeuverScaled(Args args, double dtmin = double
sw.Stop();
Print($"optimization took {sw.ElapsedMilliseconds}ms: {rep.iterationscount} iter, {rep.nfev} fev");

/*
alglib.minnlcoptguardresults(state, out alglib.optguardreport ogrep);
if (optguard)
{
alglib.minnlcoptguardresults(state, out alglib.optguardreport ogrep);

if (ogrep.badgradsuspected)
throw new Exception(
$"badgradsuspected: {ogrep.badgradfidx},{ogrep.badgradvidx}\nuser:\n{DoubleMatrixString(ogrep.badgraduser)}\nnumerical:\n{DoubleMatrixString(ogrep.badgradnum)}\nsparsity check:\n{DoubleMatrixSparsityCheck(ogrep.badgraduser, ogrep.badgradnum, 1e-2)}");
if (ogrep.badgradsuspected)
throw new Exception(
$"badgradsuspected: {ogrep.badgradfidx},{ogrep.badgradvidx}\nuser:\n{DoubleMatrixString(ogrep.badgraduser)}\nnumerical:\n{DoubleMatrixString(ogrep.badgradnum)}\nsparsity check:\n{DoubleMatrixSparsityCheck(ogrep.badgraduser, ogrep.badgradnum, 1e-2)}");

if (ogrep.nonc0suspected || ogrep.nonc1suspected)
throw new Exception("alglib optguard caught an error, i should report better on errors now");
*/
if (ogrep.nonc0suspected || ogrep.nonc1suspected)
throw new Exception("alglib optguard caught an error, i should report better on errors now");
}

if (rep.terminationtype < 0)
throw new Exception(
Expand Down Expand Up @@ -453,7 +456,8 @@ private static (V3 V, double dt) ManeuverScaled(Args args, double dtmin = double
}

private static (V3 V, double dt) Maneuver(double centralMu, double moonMu, V3 moonR0, V3 moonV0, double moonSOI,
V3 r0, V3 v0, double peR, double inc, double dtmin = double.NegativeInfinity, double dtmax = double.PositiveInfinity)
V3 r0, V3 v0, double peR, double inc, double dtmin = double.NegativeInfinity, double dtmax = double.PositiveInfinity,
bool optguard = false)
{
var sw = new Stopwatch();
sw.Start();
Expand Down Expand Up @@ -484,7 +488,7 @@ private static (V3 V, double dt) ManeuverScaled(Args args, double dtmin = double
PlanetScale = planetScale
};

(V3 dv, double dt) = ManeuverScaled(args, dtmin / moonScale.TimeScale, dtmax / moonScale.TimeScale);
(V3 dv, double dt) = ManeuverScaled(args, dtmin / moonScale.TimeScale, dtmax / moonScale.TimeScale, optguard);

sw.Stop();
Print($"total calculation took {sw.ElapsedMilliseconds}ms");
Expand All @@ -493,7 +497,8 @@ private static (V3 V, double dt) ManeuverScaled(Args args, double dtmin = double
}

public static (V3 dv, double dt, double newPeR) NextManeuver(double centralMu, double moonMu, V3 moonR0, V3 moonV0,
double moonSOI, V3 r0, V3 v0, double peR, double inc, double dtmin = double.NegativeInfinity, double dtmax = double.PositiveInfinity)
double moonSOI, V3 r0, V3 v0, double peR, double inc, double dtmin = double.NegativeInfinity, double dtmax = double.PositiveInfinity,
bool optguard = false)
{
double dt;
V3 dv;
Expand All @@ -503,7 +508,7 @@ private static (V3 V, double dt) ManeuverScaled(Args args, double dtmin = double

while (true)
{
(dv, dt) = Maneuver(centralMu, moonMu, moonR0, moonV0, moonSOI, r0, v0, peR, inc, dtmin, dtmax);
(dv, dt) = Maneuver(centralMu, moonMu, moonR0, moonV0, moonSOI, r0, v0, peR, inc, dtmin, dtmax, optguard);
if (dt > 0 || ecc >= 1)
break;
if (i++ >= 5)
Expand Down
40 changes: 37 additions & 3 deletions MechJebLibTest/ManeuversTests/ReturnFromMoonTests.cs
View file
Expand Up @@ -21,11 +21,10 @@ public ReturnFromMoonTests(ITestOutputHelper testOutputHelper)
[Fact]
private void NextManeuverToReturnFromMoonTest()
{
Logger.Register(o => _testOutputHelper.WriteLine(o.ToString()));
// this forces JIT compilation(?) of the SQL solver which takes ~250ms
ChangeOrbitalElement.ChangePeriapsis(1.0, new V3(1, 0, 0), new V3(0, 1.0, 0), 1.0);

// Logger.Register(o => _testOutputHelper.WriteLine((string)o));
Logger.Register(o => _testOutputHelper.WriteLine((string)o));
const double CENTRAL_MU = 398600435436096;
const double MOON_MU = 4902800066163.8;
var moonR0 = new V3(325420116.073166, -166367503.579338, -138858150.96145);
Expand All @@ -50,10 +49,11 @@ private void NextManeuverToReturnFromMoonTest()
Astro.ApoapsisFromStateVectors(MOON_MU, r0, v0) > MOON_SOI)
continue;

/*
r0 = new V3(3033960.04435434, -538512.74449519, -1569171.01639252);
v0 = new V3(344.550626047212, -973.108221764261, 907.813925253141);

/*
r0 = new V3(-23744.6019871556, -935848.195057236, -2970820.75826717);
v0 = new V3(-511.683969531061, -141.692448007731, 975.982327934346);
Expand Down Expand Up @@ -94,5 +94,39 @@ private void NextManeuverToReturnFromMoonTest()
newPeR.ShouldEqual(PER, 1e-3);
}
}

[Fact]
private void NextManeuverToReturnFromMoonOptguardTest()
{
// this forces JIT compilation(?) of the SQL solver which takes ~250ms
ChangeOrbitalElement.ChangePeriapsis(1.0, new V3(1, 0, 0), new V3(0, 1.0, 0), 1.0);

Logger.Register(o => _testOutputHelper.WriteLine((string)o));
const double CENTRAL_MU = 398600435436096;
const double MOON_MU = 4902800066163.8;
var moonR0 = new V3(325420116.073166, -166367503.579338, -138858150.96145);
var moonV0 = new V3(577.012296778094, 761.848508254181, 297.464594270612);
const double MOON_SOI = 66167158.6569544;

var r0 = new V3(4198676.73768844, 5187520.71497923, -3.29371833446352);
var v0 = new V3(-666.230112925872, 539.234048888927, 0.000277598267012666);

const double PER = 6.3781e6 + 60000; // 60km

(V3 dv, double dt, double newPeR) =
ReturnFromMoon.NextManeuver(398600435436096, 4902800066163.8, moonR0, moonV0, 66167158.6569544, r0, v0, PER, 0, 0, optguard: true);

(V3 r1, V3 v1) = Shepperd.Solve(MOON_MU, dt, r0, v0);
double tt1 = Astro.TimeToNextRadius(MOON_MU, r1, v1 + dv, MOON_SOI);
(V3 r2, V3 v2) = Shepperd.Solve(MOON_MU, tt1, r1, v1 + dv);
(V3 moonR2, V3 moonV2) = Shepperd.Solve(CENTRAL_MU, dt + tt1, moonR0, moonV0);
V3 r3 = moonR2 + r2;
V3 v3 = moonV2 + v2;

_testOutputHelper.WriteLine($"periapsis: {Astro.PeriapsisFromStateVectors(CENTRAL_MU, r3, v3)}");

Astro.PeriapsisFromStateVectors(CENTRAL_MU, r3, v3).ShouldEqual(PER, 1e-3);
newPeR.ShouldEqual(PER, 1e-3);
}
}
}

0 comments on commit bf9e8dc

Please sign in to comment.