From dc1bf95d22dcc0951376f72a8c9aa43cf764850f Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 23 Aug 2023 16:18:10 -0700 Subject: [PATCH] Convert ReturnFromMoon maneuver to use analytical Jacobian MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This doesn't seem to do that much in terms of speedup or stability really (although I didn't try investigating stability very well). Investigating the values in the jacobian one of the biggest problems is clearly integrating back from the terminal endpoints. The biggest numbers come from the state transition matrix there. This is likely because small target periapsis constraints results in highly eccentric return orbits. Taking problems that blow up and using a higher target periapsis often results in convergence. There is a note in Ellison & Englander (2019) that might be applicable: > A more natural propagation strategy would be to utilize time > eegularization, such a Sundman transformation,18–20 and a > corresponding modification to the variational equations. This > is left as future work. It might also be possible to use some kind of "homotopy" and if the problem fails, relax the periapsis constraint to some higher intermediate point that is easier to solve, then use that as an initial guess. I'd really like to avoid iterative approaches. Also I have not investigated how good/bad my initial guessing is, and it may be possible that in the cases which do not converge that it is very poor. I think it may also work to target an Apoapsis now (higher orbit than the moon around the primary), although I didn't test that a lot. Signed-off-by: Lamont Granquist --- .../Maneuvers/ChangeOrbitalElement.cs | 8 + .../MechJebLib/Maneuvers/ReturnFromMoon.cs | 225 ++++++++++++++---- MechJeb2/MechJebLib/Primitives/DualV3.cs | 3 +- MechJeb2/MechJebLib/Primitives/M3.cs | 7 + MechJeb2/MechJebLib/Primitives/V3.cs | 9 + MechJeb2/MechJebLib/Utils/Statics.cs | 59 +++++ MechJebLibTest/Maths/FunctionsTests.cs | 9 +- 7 files changed, 273 insertions(+), 47 deletions(-) diff --git a/MechJeb2/MechJebLib/Maneuvers/ChangeOrbitalElement.cs b/MechJeb2/MechJebLib/Maneuvers/ChangeOrbitalElement.cs index 39377374..06f79bd0 100644 --- a/MechJeb2/MechJebLib/Maneuvers/ChangeOrbitalElement.cs +++ b/MechJeb2/MechJebLib/Maneuvers/ChangeOrbitalElement.cs @@ -194,6 +194,14 @@ public static V3 ChangeApoapsis(double mu, V3 r, V3 v, double apR, bool optguard return DeltaV(mu, r, v, apR, Type.APOAPSIS, optguard); } + public static V3 ChangeApsis(double mu, V3 r, V3 v, double valueR, bool optguard = false) + { + if (!valueR.IsFinite()) + throw new ArgumentException($"Bad apoapsis in ChangeOrbitalElement = {valueR}"); + + return valueR <= r.magnitude ? DeltaV(mu, r, v, valueR, Type.PERIAPSIS, optguard) : DeltaV(mu, r, v, valueR, Type.APOAPSIS, optguard); + } + public static V3 ChangeSMA(double mu, V3 r, V3 v, double sma, bool optguard = false) { if (!sma.IsFinite()) diff --git a/MechJeb2/MechJebLib/Maneuvers/ReturnFromMoon.cs b/MechJeb2/MechJebLib/Maneuvers/ReturnFromMoon.cs index cc8e42fd..02aeb4a9 100644 --- a/MechJeb2/MechJebLib/Maneuvers/ReturnFromMoon.cs +++ b/MechJeb2/MechJebLib/Maneuvers/ReturnFromMoon.cs @@ -29,12 +29,13 @@ private struct Args public Scale MoonToPlanetScale; } - private static void NLPFunction(double[] x, double[] fi, object obj) + private static void NLPFunction(double[] x, double[] fi, double[,] jac, object o) + //private static void NLPFunction(double[] x, double[] fi, object obj) { /* * unpacking constants */ - var args = (Args)obj; + var args = (Args)o; double moonSOI = args.MoonSOI; double peR = args.PeR; @@ -54,54 +55,171 @@ private static void NLPFunction(double[] x, double[] fi, object obj) double moonDt2 = x[5]; V3 rsoi = new V3(moonSOI, x[6], x[7]).sph2cart; V3 vsoi = new V3(x[8], x[9], x[10]).sph2cart; - double planetDt1 = x[11]; - double planetDt2 = x[12]; - var rf = new V3(x[13], x[14], x[15]); - var vf = new V3(x[16], x[17], x[18]); + double moonDt = x[11]; + double planetDt1 = x[12]; + double planetDt2 = x[13]; + var rf = new V3(x[14], x[15], x[16]); + var vf = new V3(x[17], x[18], x[19]); // forward propagation in the moon SOI (V3 rburn, V3 vburn) = Shepperd.Solve(1.0, burnTime, r0, v0); - (V3 r1Minus, V3 v1Minus) = Shepperd.Solve(1.0, moonDt1, rburn, vburn + dv); + V3 aburn = -rburn / (rburn.sqrMagnitude * rburn.magnitude); + (V3 r1Minus, V3 v1Minus, M3 v1MinusS00, M3 v1MinusS01, M3 v1MinusS10, M3 v1MinusS11) = Shepperd.Solve2(1.0, moonDt1, rburn, vburn + dv); + V3 a1Minus = -r1Minus / (r1Minus.sqrMagnitude * r1Minus.magnitude); // reverse propagation in the moon SOI - (V3 r1Plus, V3 v1Plus) = Shepperd.Solve(1.0, -moonDt2, rsoi, vsoi); + (V3 r1Plus, V3 v1Plus, M3 v1PlusS00, M3 v1PlusS01, M3 v1PlusS10, M3 v1PlusS11) = Shepperd.Solve2(1.0, -moonDt2, rsoi, vsoi); + V3 a1Plus = -r1Plus / (r1Plus.sqrMagnitude * r1Plus.magnitude); // propagation of the moon via the "ephemeris" - double t2 = (moonDt1 + moonDt2 + burnTime) / moonToPlanetScale.TimeScale; + double t2 = moonDt / moonToPlanetScale.TimeScale; (V3 moonR2, V3 moonV2) = Shepperd.Solve(1.0, t2, moonR0, moonV0); + V3 moonA2 = -moonR2 / (moonR2.sqrMagnitude * moonR2.magnitude); V3 rsoiPlanet = rsoi / moonToPlanetScale.LengthScale + moonR2; V3 vsoiPlanet = vsoi / moonToPlanetScale.VelocityScale + moonV2; // forward propagation in the planet SOI - (V3 r2Minus, V3 v2Minus) = Shepperd.Solve(1.0, planetDt1, rsoiPlanet, vsoiPlanet); + (V3 r2Minus, V3 v2Minus, M3 v2MinusS00, M3 v2MinusS01, M3 v2MinusS10, M3 v2MinusS11) = Shepperd.Solve2(1.0, planetDt1, rsoiPlanet, vsoiPlanet); + V3 a2Minus = -r2Minus / (r2Minus.sqrMagnitude * r2Minus.magnitude); // reverse propagation in the planet SOI - (V3 r2Plus, V3 v2Plus) = Shepperd.Solve(1.0, -planetDt2, rf, vf); + (V3 r2Plus, V3 v2Plus, M3 v2PlusS00, M3 v2PlusS01, M3 v2PlusS10, M3 v2PlusS11) = Shepperd.Solve2(1.0, -planetDt2, rf, vf); + V3 a2Plus = -r2Plus / (r2Plus.sqrMagnitude * r2Plus.magnitude); // objective - fi[0] = dv.sqrMagnitude; + Dual obj = new DualV3(dv, new V3(1, 0, 0)).sqrMagnitude; + fi[0] = obj.M; + jac[0, 0] = obj.D; + jac[0, 1] = new DualV3(dv, new V3(0, 1, 0)).sqrMagnitude.D; + jac[0, 2] = new DualV3(dv, new V3(0, 0, 1)).sqrMagnitude.D; + + /* + * meet in the moon's SOI + */ - // meet in the moon's SOI fi[1] = r1Minus.x - r1Plus.x; fi[2] = r1Minus.y - r1Plus.y; fi[3] = r1Minus.z - r1Plus.z; fi[4] = v1Minus.x - v1Plus.x; fi[5] = v1Minus.y - v1Plus.y; fi[6] = v1Minus.z - v1Plus.z; - // meet in the planet's SOI - fi[7] = r2Minus.x - r2Plus.x; - fi[8] = r2Minus.y - r2Plus.y; - fi[9] = r2Minus.z - r2Plus.z; - fi[10] = v2Minus.x - v2Plus.x; - fi[11] = v2Minus.y - v2Plus.y; - fi[12] = v2Minus.z - v2Plus.z; - // end at the periapsis - fi[13] = V3.Dot(rf.normalized, vf.normalized); - fi[14] = rf.magnitude * rf.magnitude - peR * peR; - // put some constraints on when the midpoints meet - fi[15] = moonDt1 - moonDt2; - fi[16] = planetDt1 - planetDt2; + + v1MinusS01.CopyTo(jac, 1, 0); + v1MinusS11.CopyTo(jac, 4, 0); + + (v1MinusS00 * vburn + v1MinusS01 * aburn).CopyTo(jac, 1,3); + (v1MinusS10 * vburn + v1MinusS11 * aburn).CopyTo(jac, 4,3); + + v1Minus.CopyTo(jac, 1, 4); + a1Minus.CopyTo(jac, 4, 4); + + v1Plus.CopyTo(jac, 1, 5); + a1Plus.CopyTo(jac, 4, 5); + + DualV3 rsoi6 = new DualV3(moonSOI, x[6], x[7], 0, 1, 0).sph2cart; + DualV3 rsoi7 = new DualV3(moonSOI, x[6], x[7], 0, 0, 1).sph2cart; + + var d2 = new M3(rsoi6.D, rsoi7.D, V3.zero); + + DualV3 vsoi8 = new DualV3(x[8], x[9], x[10], 1, 0, 0).sph2cart; + DualV3 vsoi9 = new DualV3(x[8], x[9], x[10], 0, 1, 0).sph2cart; + DualV3 vsoi10 = new DualV3(x[8], x[9], x[10], 0, 0, 1).sph2cart; + + var d3 = new M3(vsoi8.D, vsoi9.D, vsoi10.D); + + (-v1PlusS00*d2).CopyTo(jac, 1, 6); + (-v1PlusS01*d3).CopyTo(jac, 1, 8); + (-v1PlusS10*d2).CopyTo(jac, 4, 6); + (-v1PlusS11*d3).CopyTo(jac, 4, 8); + + /* + * meet in the planet's SOI + */ + + fi[7] = r2Minus.x - r2Plus.x; + fi[8] = r2Minus.y - r2Plus.y; + fi[9] = r2Minus.z - r2Plus.z; + fi[10] = v2Minus.x - v2Plus.x; + fi[11] = v2Minus.y - v2Plus.y; + fi[12] = v2Minus.z - v2Plus.z; + + double l = moonToPlanetScale.LengthScale; + double v = moonToPlanetScale.VelocityScale; + double t = moonToPlanetScale.TimeScale; + + DualV3 rsoiPlanet6 = new DualV3(moonSOI, x[6], x[7], 0, 1, 0).sph2cart/ l + moonR2; + DualV3 rsoiPlanet7 = new DualV3(moonSOI, x[6], x[7], 0, 0, 1).sph2cart/ l + moonR2; + + var d0 = new M3(rsoiPlanet6.D, rsoiPlanet7.D, V3.zero); + + DualV3 vsoiPlanet8 = new DualV3(x[8], x[9], x[10], 1, 0, 0).sph2cart / v + moonV2; + DualV3 vsoiPlanet9 = new DualV3(x[8], x[9], x[10], 0, 1, 0).sph2cart / v + moonV2; + DualV3 vsoiPlanet10 = new DualV3(x[8], x[9], x[10], 0, 0, 1).sph2cart / v + moonV2; + + var d1 = new M3(vsoiPlanet8.D, vsoiPlanet9.D, vsoiPlanet10.D); + + (v2MinusS00*d0).CopyTo(jac, 7, 6); + (v2MinusS01*d1).CopyTo(jac, 7, 8); + (v2MinusS10*d0).CopyTo(jac, 10, 6); + (v2MinusS11*d1).CopyTo(jac, 10, 8); + + ((v2MinusS00 * moonV2 + v2MinusS01 * moonA2) / t).CopyTo(jac, 7,11); + ((v2MinusS10 * moonV2 + v2MinusS11 * moonA2) / t).CopyTo(jac, 10,11); + + v2Minus.CopyTo(jac, 7, 12); + a2Minus.CopyTo(jac, 10, 12); + + v2Plus.CopyTo(jac, 7, 13); + a2Plus.CopyTo(jac, 10, 13); + + (-v2PlusS00).CopyTo(jac, 7, 14); + (-v2PlusS01).CopyTo(jac, 7, 17); + (-v2PlusS10).CopyTo(jac, 10, 14); + (-v2PlusS11).CopyTo(jac, 10, 17); + + /* + * periapsis condition + */ + + Dual p = DualV3.Dot(new DualV3(rf, new V3(1, 0, 0)).normalized, vf.normalized); + 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; + + /* + * periapsis constraint + */ + + fi[14] = rf.sqrMagnitude - peR * peR; + jac[14, 14] = (new DualV3(rf, new V3(1, 0, 0)).sqrMagnitude - peR * peR).D; + jac[14, 15] = (new DualV3(rf, new V3(0, 1, 0)).sqrMagnitude - peR * peR).D; + jac[14, 16] = (new DualV3(rf, new V3(0, 0, 1)).sqrMagnitude - peR * peR).D; + + /* + * time constraints + */ + + fi[15] = moonDt1 + moonDt2 + burnTime - moonDt; + jac[15, 3] = 1.0; + jac[15, 4] = 1.0; + jac[15, 5] = 1.0; + jac[15, 11] = -1.0; + + /* + * midpoint time constraints + */ + + fi[16] = moonDt1 - moonDt2; + jac[16, 4] = 1.0; + jac[16, 5] = -1.0; + fi[17] = planetDt1 - planetDt2; + jac[17, 12] = 1.0; + jac[17, 13] = -1.0; } [UsedImplicitly] @@ -114,7 +232,7 @@ private static void NLPFunction(double[] x, double[] fi, object obj) const double EPSX = 1e-4; const int MAXITS = 1000; - const int NVARIABLES = 19; + const int NVARIABLES = 20; const int NEQUALITYCONSTRAINTS = 17; const int NINEQUALITYCONSTRAINTS = 0; @@ -144,7 +262,7 @@ private static void NLPFunction(double[] x, double[] fi, object obj) if (ecc < 1) { // approximate vsoi with the amount to kick the moon's orbit down to the periapsis - V3 v1 = ChangeOrbitalElement.ChangePeriapsis(centralMu, moonR0, moonV0, peR); + V3 v1 = ChangeOrbitalElement.ChangeApsis(centralMu, moonR0, moonV0, peR); // then do the source moon SOI V3 vneg, vpos, rburn; @@ -167,9 +285,8 @@ private static void NLPFunction(double[] x, double[] fi, object obj) // propagate the moon forward to the estimate of the time of the burn (V3 moonR2, V3 moonV2) = Shepperd.Solve(centralMu, dt, moonR0, moonV0); - // construct rf + vf by propagating the moon forward to the burn time and changing the PeR to find a - // somewhat feasible rf+vf - V3 moonV3 = moonV2 + ChangeOrbitalElement.ChangePeriapsis(centralMu, moonR2, moonV2, peR); + // construct rf + vf by propagating the moon forward to the burn time and changing the PeR to find a feasible rf+vf + V3 moonV3 = moonV2 + ChangeOrbitalElement.ChangeApsis(centralMu, moonR2, moonV2, peR); double tt2 = Maths.TimeToNextPeriapsis(centralMu, moonR0, moonV3); (rf, vf) = Shepperd.Solve(centralMu, tt2, moonR0, moonV3); @@ -192,14 +309,15 @@ private static void NLPFunction(double[] x, double[] fi, object obj) x[8] = v2Sph[0]; // r of SOI velocity x[9] = v2Sph[1]; // theta of SOI velocity x[10] = v2Sph[2]; // phi of SOI velocity - x[11] = tt2 / 2; // 1/2 coast time through planet SOI - x[12] = tt2 / 2; // 1/2 coast time thorugh planet SOI - x[13] = rf.x; // final rx - x[14] = rf.y; // final ry - x[15] = rf.z; // final rz - x[16] = vf.x; // final vx - x[17] = vf.y; // final vy - x[18] = vf.z; // final vz + x[11] = tt1; // coast time through moon SOI + x[12] = tt2 / 2; // 1/2 coast time through planet SOI + x[13] = tt2 / 2; // 1/2 coast time thorugh planet SOI + x[14] = rf.x; // final rx + x[15] = rf.y; // final ry + x[16] = rf.z; // final rz + x[17] = vf.x; // final vx + x[18] = vf.y; // final vy + x[19] = vf.z; // final vz var args = new Args { @@ -210,28 +328,45 @@ private static void NLPFunction(double[] x, double[] fi, object obj) V0 = v0 / moonScale.VelocityScale, MoonR0 = moonR0 / planetScale.LengthScale, MoonV0 = moonV0 / planetScale.VelocityScale, - MoonToPlanetScale = moonToPlanetScale, + MoonToPlanetScale = moonToPlanetScale }; - alglib.minnlccreatef(NVARIABLES, x, DIFFSTEP, out alglib.minnlcstate state); + //alglib.minnlccreatef(NVARIABLES, x, DIFFSTEP, out alglib.minnlcstate state); + alglib.minnlccreate(NVARIABLES, x, out alglib.minnlcstate state); + alglib.minnlcsetbc(state, bndl, bndu); alglib.minnlcsetstpmax(state, 1e-8); alglib.minnlcsetalgosqp(state); alglib.minnlcsetcond(state, EPSX, MAXITS); alglib.minnlcsetnlc(state, NEQUALITYCONSTRAINTS, NINEQUALITYCONSTRAINTS); + + /* + alglib.minnlcoptguardsmoothness(state); + alglib.minnlcoptguardgradient(state, DIFFSTEP); + */ + alglib.minnlcoptimize(state, NLPFunction, null, args); alglib.minnlcresults(state, out x, out alglib.minnlcreport rep); + /* + 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.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( $"ReturnFromMoon.Maneuver(): SQP solver terminated abnormally: {rep.terminationtype}" ); - NLPFunction(x, fi, args); - fi[0] = 0; // zero out the objective - if (DoubleArrayMagnitude(fi) > 1e-4) + if (rep.nlcerr > 1e-4) throw new Exception( - $"ReturnFromMoon.Maneuver() no feasible solution found: constraint violation: {DoubleArrayMagnitude(fi)}"); + $"ReturnFromMoon.Maneuver() no feasible solution found, constraint violation: {rep.nlcerr}"); return (new V3(x[0], x[1], x[2]) * moonScale.VelocityScale, x[3] * moonScale.TimeScale); } diff --git a/MechJeb2/MechJebLib/Primitives/DualV3.cs b/MechJeb2/MechJebLib/Primitives/DualV3.cs index eda3b741..f685d54d 100644 --- a/MechJeb2/MechJebLib/Primitives/DualV3.cs +++ b/MechJeb2/MechJebLib/Primitives/DualV3.cs @@ -18,7 +18,7 @@ public class DualV3 : IEquatable, IComparable, IFormattable 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) + public DualV3(V3 m, V3? dx = null) { M = m; D = dx ?? V3.zero; @@ -30,6 +30,7 @@ private DualV3(V3 m, V3? dx = null) 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 DualV3 sph2cart => x * new DualV3(Dual.Cos(z) * Dual.Sin(y), Dual.Sin(z) * Dual.Sin(y), Dual.Cos(y)); public static DualV3 operator +(DualV3 a, DualV3 b) => new DualV3(a.x + b.x, a.y + b.y, a.z + b.z); diff --git a/MechJeb2/MechJebLib/Primitives/M3.cs b/MechJeb2/MechJebLib/Primitives/M3.cs index a37abd56..78052a74 100644 --- a/MechJeb2/MechJebLib/Primitives/M3.cs +++ b/MechJeb2/MechJebLib/Primitives/M3.cs @@ -563,6 +563,13 @@ public double min_magnitude } } + public void CopyTo(double[,] other, int x, int y) + { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + other[x + i, y + j] = this[i, j]; + } + // TODO: // private QuaternionD GetRotation(); // public static M3 LookAt(V3 from, V3 to, V3 up); diff --git a/MechJeb2/MechJebLib/Primitives/V3.cs b/MechJeb2/MechJebLib/Primitives/V3.cs index 5f768440..96831ef3 100644 --- a/MechJeb2/MechJebLib/Primitives/V3.cs +++ b/MechJeb2/MechJebLib/Primitives/V3.cs @@ -351,6 +351,8 @@ public V3 cart2sph public static V3 operator /(V3 a, double d) => new V3(a.x / d, a.y / d, a.z / d); + public static V3 operator /(double d, V3 a) => new V3(d/a.x, d/a.y, d/a.z); + public static bool operator ==(V3 lhs, V3 rhs) { double diff_X = lhs.x - rhs.x; @@ -389,5 +391,12 @@ public void CopyTo(IList other, int index = 0) other[index + 1] = this[1]; other[index + 2] = this[2]; } + + public void CopyTo(double[,] other, int i, int j) + { + other[i,j] = this[0]; + other[i + 1,j] = this[1]; + other[i + 2,j] = this[2]; + } } } diff --git a/MechJeb2/MechJebLib/Utils/Statics.cs b/MechJeb2/MechJebLib/Utils/Statics.cs index 0e2ca3d9..aae4ce13 100644 --- a/MechJeb2/MechJebLib/Utils/Statics.cs +++ b/MechJeb2/MechJebLib/Utils/Statics.cs @@ -374,6 +374,65 @@ public static string DoubleArrayString(IList array) return sb.ToString(); } + public static string DoubleArraySparsity(IList user, IList numerical, double tol) + { + var sb = new StringBuilder(); + + sb.Append("["); + int last = user.Count - 1; + + for (int i = 0; i <= last; i++) + { + if (user[i] == 0 && numerical[i] == 0) + sb.Append("⚫"); // zero in both + else if (NearlyEqual(user[i], numerical[i], tol)) + sb.Append("✅"); // agrees + else if (user[i] == 0) + sb.Append("❗"); // not done yet + else + sb.Append("❌"); // mistake + if (i < last) + sb.Append(","); + } + + sb.Append("]"); + + + return sb.ToString(); + } + + public static string DoubleMatrixString(double[,] matrix) + { + var sb = new StringBuilder(); + + for (int i = 0; i <= matrix.GetUpperBound(0); i++) + sb.AppendLine(DoubleArrayString(GetRow(matrix, i))); + + return sb.ToString(); + } + + public static string DoubleMatrixSparsityCheck(double[,] user, double[,] numerical, double tol) + { + var sb = new StringBuilder(); + + for (int i = 0; i <= user.GetUpperBound(0); i++) + sb.AppendLine(DoubleArraySparsity(GetRow(user, i), GetRow(numerical, i), tol)); + + return sb.ToString(); + } + + public static double[] GetRow(double[,] array, int row) + { + int cols = array.GetUpperBound(1) + 1; + double[] result = new double[cols]; + + int size = sizeof(double); + + Buffer.BlockCopy(array, row * cols * size, result, 0, cols * size); + + return result; + } + public static double DoubleArrayMagnitude(IList array) { int last = array.Count - 1; diff --git a/MechJebLibTest/Maths/FunctionsTests.cs b/MechJebLibTest/Maths/FunctionsTests.cs index ed4d663e..f2a60ad5 100644 --- a/MechJebLibTest/Maths/FunctionsTests.cs +++ b/MechJebLibTest/Maths/FunctionsTests.cs @@ -852,8 +852,9 @@ private void NextManeuverToReturnFromMoonTest() double moonSOI = 66167158.6569544; var r0 = new V3(4198676.73768844, 5187520.71497923, -3.29371833446352); var v0 = new V3(-666.230112925872, 539.234048888927, 0.000277598267012666); - double peR = 6471000; //6.3781e6 + 60000; + double peR = 6.3781e6 + 60000000; // 60Mm + // this test periodically just fails, although its about 1-in-30 right now for (int i = 0; i < 1; i++) { var random = new Random(); @@ -862,6 +863,12 @@ private void NextManeuverToReturnFromMoonTest() v0 = new V3(2000 * random.NextDouble() - 1000, 2000 * random.NextDouble() - 1000, 2000 * random.NextDouble() - 1000); /* + r0 = new V3(1448096.6091073, 2242802.60448088, 593799.176165359); + v0 = new V3(316.818652356425, -684.168486708854, -453.106628476226); + + r0 = new V3(72567.8252483425, -1202508.27881747, 995820.818247795); + v0 = new V3(733.61121478193, -501.358138165138, -26.4032981481417); + r0 = new V3(1105140.06213014, -8431008.05414815, -4729658.84487529); v0 = new V3(-293.374690393787, -1173.3597686151, -411.755491683683); */