More cleanup and reorg

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>

MechJebLib/Lambert/Izzo.cs

@@ -6,11 +6,10 @@
 using System;
 using System.Runtime.CompilerServices;
 using MechJebLib.Primitives;
+using MechJebLib.Utils;
 using static System.Math;
 using static MechJebLib.Utils.Statics;
 
-#nullable enable
-
 namespace MechJebLib.Lambert
 {
     /// <summary>
@@ -25,7 +24,7 @@ public class Izzo
     {
         public (V3 Vi, V3 Vf) Solve(double mu, V3 r1, V3 v1, V3 r2, double tof, int nrev)
         {
-            lambert_problem2(r1, v1, r2, tof, mu, nrev);
+            Solve2(mu, r1, v1, r2, tof, nrev);
 
             if (_nsol == 0)
                 return (_v1[0], _v2[0]);
@@ -53,62 +52,48 @@ public class Izzo
         private double[] _x     = new double[5];
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private void lambert_problem2(V3 r1, V3 v1, V3 r2, double tof, double mu, int nrev)
+        public void Solve2(double mu, V3 r1, V3 v1, V3 r2, double tof, int nrev)
         {
             V3 hhat = V3.Cross(r1, v1).normalized;
             V3 zhat = V3.Cross(r1.normalized, r2.normalized).normalized;
 
             /* if angle to orbit normal is greater than 90 degrees, then flip the orbit normal */
             bool flip = V3.Dot(hhat, zhat) < 0;
 
-            lambert_problem(r1, r2, tof, mu, nrev, flip);
+            Solve3(mu, r1, r2, tof, nrev, flip);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private void lambert_problem(V3 r1, V3 r2, double tof, double mu, int nrev, bool flip = false)
+        public void Solve3(double mu, V3 r1, V3 r2, double tof, int nrev, bool flip = false)
         {
-            // 0 - Sanity checks
-            if (tof <= 0)
-                throw new Exception("Time of flight is negative!");
-
-            if (mu <= 0)
-                throw new Exception("Gravity parameter is zero or negative!");
+            Check.PositiveFinite(tof);
+            Check.PositiveFinite(mu);
 
-            // 1 - Getting lambda and T
+            // calculate lambda and normalized tof
             double c = (r2 - r1).magnitude;
             double r1M = r1.magnitude;
             double r2M = r2.magnitude;
             double s = (c + r1M + r2M) / 2.0;
-            V3 ir1 = r1.normalized;
-            V3 ir2 = r2.normalized;
-            V3 ih = V3.Cross(ir1, ir2).normalized;
-
             double lambda2 = 1.0 - c / s;
             double lambda = Sqrt(lambda2);
+            double lambda3 = lambda * lambda2;
+            double t = Sqrt(2.0 * mu / s / s / s) * tof;
 
-            var it1 = V3.Cross(ih, ir1);
-            var it2 = V3.Cross(ih, ir2);
-
-            it1 = it1.normalized;
-            it2 = it2.normalized;
+            // unit vectors
+            V3 ir1 = r1.normalized;
+            V3 ir2 = r2.normalized;
+            V3 ih = V3.Cross(ir1, ir2).normalized;
+            V3 it1 = V3.Cross(ih, ir1).normalized;
+            V3 it2 = V3.Cross(ih, ir2).normalized;
 
             if (flip)
             {
-                // Retrograde motion
                 lambda = -lambda;
-                it1[0] = -it1[0];
-                it1[1] = -it1[1];
-                it1[2] = -it1[2];
-                it2[0] = -it2[0];
-                it2[1] = -it2[1];
-                it2[2] = -it2[2];
+                it1    = -it1;
+                it2    = -it2;
             }
 
-            double lambda3 = lambda * lambda2;
-            double t = Sqrt(2.0 * mu / s / s / s) * tof;
-
-            // 2 - We now have lambda, T and we will find all x
-            // 2.1 - Let us first detect the maximum number of revolutions for which there exists a solution
+            // compute nmax;
             int nmax = (int)(t / PI);
             double t00 = Acos(lambda) + lambda * Sqrt(1.0 - lambda2);
             double t0 = t00 + nmax * PI;
@@ -131,7 +116,7 @@ private void lambert_problem(V3 r1, V3 r2, double tof, double mu, int nrev, bool
                         if (err < 1e-13 || it > 12)
                             break;
 
-                        tMin = X2Tof(lambda, xNew, nmax);
+                        tMin = TimeOfFlight(lambda, xNew, nmax);
                         xOld = xNew;
                         it++;
                     }
@@ -141,12 +126,11 @@ private void lambert_problem(V3 r1, V3 r2, double tof, double mu, int nrev, bool
                 }
             }
 
-            // We exit this if clause with Nmax being the maximum number of revolutions
-            // for which there exists a solution. We crop it to m_multi_revs
+            // clip nmax to nrev
             nmax  = Min(nrev, nmax);
             _nsol = nmax * 2 + 1;
 
-            // 2.2 We now allocate the memory for the output variables
+            // extend memory if necessary
             if (_nsol > _x.Length)
             {
                 _v1    = new V3[nmax * 2 + 1];
@@ -155,32 +139,29 @@ private void lambert_problem(V3 r1, V3 r2, double tof, double mu, int nrev, bool
                 _x     = new double[nmax * 2 + 1];
             }
 
-            // 3 - We may now find all solutions in x,y
-            // 3.1 0 rev solution
-            // 3.1.1 initial guess
+            // initial guess
             if (t >= t00)
                 _x[0] = -(t - t00) / (t - t00 + 4);
             else if (t <= t1)
                 _x[0] = t1 * (t1 - t) / (2.0 / 5.0 * (1 - lambda2 * lambda3) * t) + 1;
             else
                 _x[0] = Pow(t / t00, 0.69314718055994529 / Log(t1 / t00)) - 1.0;
 
-            // 3.1.2 Householder iterations
+            // householder iteration for all revolutions
             _iters[0] = Householder(lambda, t, ref _x[0], 0, 1e-5, 15);
-            // 3.2 multi rev solutions
             for (int i = 1; i < nmax + 1; ++i)
             {
-                // 3.2.1 left Householder iterations
+                // left householder
                 double tmp = Pow((i * PI + PI) / (8.0 * t), 2.0 / 3.0);
                 _x[2 * i - 1]     = (tmp - 1) / (tmp + 1);
                 _iters[2 * i - 1] = Householder(lambda, t, ref _x[2 * i - 1], i, 1e-8, 15);
-                // 3.2.1 right Householder iterations
+                // right householder
                 tmp           = Pow(8.0 * t / (i * PI), 2.0 / 3.0);
                 _x[2 * i]     = (tmp - 1) / (tmp + 1);
                 _iters[2 * i] = Householder(lambda, t, ref _x[2 * i], i, 1e-8, 15);
             }
 
-            // 4 - For each found x value we reconstruct the terminal velocities
+            // build the velocities for all revolutions
             double gamma = Sqrt(mu * s / 2.0);
             double rho = (r1M - r2M) / c;
             double sigma = Sqrt(1 - rho * rho);
@@ -206,7 +187,7 @@ private static int Householder(double lambda, double t, ref double x0, int n, do
             double err = 1.0;
             while (err > eps && it < maxIter)
             {
-                double tof = X2Tof(lambda, x0, n);
+                double tof = TimeOfFlight(lambda, x0, n);
                 (double dt, double ddt, double dddt) = DTdx(lambda, x0, tof);
                 double delta = tof - t;
                 double dt2 = dt * dt;
@@ -235,50 +216,30 @@ private static ( double DT, double DDT, double DDDT) DTdx(double lambda, double
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static double X2Tof2(double lambda, double x, int n)
-        {
-            double a = 1.0 / (1.0 - x * x);
-            if (a > 0) // ellipse
-            {
-                double alfa = 2.0 * Acos(x);
-                double beta = 2.0 * Asin(Sqrt(lambda * lambda / a));
-                if (lambda < 0.0) beta = -beta;
-                return a * Sqrt(a) * (alfa - Sin(alfa) - (beta - Sin(beta)) + 2.0 * PI * n) / 2.0;
-            }
-            else
-            {
-                double alfa = 2.0 * Acosh(x);
-                double beta = 2.0 * Asinh(Sqrt(-lambda * lambda / a));
-                if (lambda < 0.0) beta = -beta;
-                return -a * Sqrt(-a) * (beta - Sinh(beta) - (alfa - Sinh(alfa))) / 2.0;
-            }
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static double X2Tof(double lambda, double x, int n)
+        private static double TimeOfFlight(double lambda, double x, int n)
         {
             const double BATTIN = 0.01;
             const double LAGRANGE = 0.2;
             double dist = Abs(x - 1);
             if (dist < LAGRANGE && dist > BATTIN)
-                // We use Lagrange tof expression
-                return X2Tof2(lambda, x, n);
+                // use lagrange expression
+                return TimeOfFlightLagrange(lambda, x, n);
 
             double k = lambda * lambda;
             double e = x * x - 1.0;
             double rho = Abs(e);
             double z = Sqrt(1 + k * e);
             if (dist < BATTIN)
             {
-                // We use Battin series tof expression
+                // use battin expression
                 double eta = z - lambda * x;
                 double s1 = 0.5 * (1.0 - lambda - x * eta);
                 double q = HypergeometricF(s1, 1e-11);
                 q = 4.0 / 3.0 * q;
                 return (eta * eta * eta * q + 4.0 * lambda * eta) / 2.0 + n * PI / Pow(rho, 1.5);
             }
 
-            // We use Lancaster tof expresion
+            // use lancaster expression
             double y = Sqrt(rho);
             double g = x * z - lambda * e;
             double d;
@@ -296,6 +257,27 @@ private static double X2Tof(double lambda, double x, int n)
             return (x - lambda * z - d / y) / e;
         }
 
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static double TimeOfFlightLagrange(double lambda, double x, int n)
+        {
+            double a = 1.0 / (1.0 - x * x);
+            if (a > 0) // ellipse
+            {
+                double alfa = 2.0 * Acos(x);
+                double beta = 2.0 * Asin(Sqrt(lambda * lambda / a));
+                if (lambda < 0.0) beta = -beta;
+                return a * Sqrt(a) * (alfa - Sin(alfa) - (beta - Sin(beta)) + 2.0 * PI * n) / 2.0;
+            }
+            else
+            {
+                double alfa = 2.0 * Acosh(x);
+                double beta = 2.0 * Asinh(Sqrt(-lambda * lambda / a));
+                if (lambda < 0.0) beta = -beta;
+                return -a * Sqrt(-a) * (beta - Sinh(beta) - (alfa - Sinh(alfa))) / 2.0;
+            }
+        }
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         private static double HypergeometricF(double z, double tol)
         {

MechJebLibTest/AssertionExtensions.cs

@@ -11,7 +11,7 @@
 using static MechJebLib.Utils.Statics;
 using static System.Math;
 
-namespace AssertExtensions
+namespace MechJebLibTest
 {
     /// <summary>
     ///     Xunit Assertion Extensions

MechJebLibTest/ControlTests/PIDLoopTests.cs

@@ -1,7 +1,7 @@
 using MechJebLib.Control;
 using Xunit;
 
-namespace ControlTests
+namespace MechJebLibTest.ControlTests
 {
     public class PIDLoopTests
     {

MechJebLibTest/MathsTests/FunctionsTests.cs → MechJebLibTest/FunctionsTests.cs

@@ -5,7 +5,6 @@
  */
 
 using System;
-using AssertExtensions;
 using MechJebLib.Functions;
 using MechJebLib.Primitives;
 using MechJebLib.TwoBody;

MechJebLibTest/MathsTests/GoodingTests.cs → MechJebLibTest/LambertTests/GoodingTests.cs

@@ -1,6 +1,5 @@
 using System;
 using System.Collections.Generic;
-using AssertExtensions;
 using MechJebLib.Functions;
 using MechJebLib.Lambert;
 using MechJebLib.Primitives;
@@ -10,7 +9,7 @@
 using static MechJebLib.Utils.Statics;
 using static System.Math;
 
-namespace MechJebLibTest.MathsTests
+namespace MechJebLibTest.LambertTests
 {
     public class GoodingTests
     {

MechJebLibTest/ManeuversTests/ChangeOrbitalElementTests.cs

@@ -1,5 +1,4 @@
 using System;
-using AssertExtensions;
 using MechJebLib.Functions;
 using MechJebLib.Maneuvers;
 using MechJebLib.Primitives;

MechJebLibTest/ManeuversTests/ReturnFromMoonTests.cs

@@ -1,5 +1,4 @@
 using System;
-using AssertExtensions;
 using MechJebLib.Functions;
 using MechJebLib.Maneuvers;
 using MechJebLib.Primitives;

MechJebLibTest/ManeuversTests/Simple.cs

@@ -1,5 +1,4 @@
 using System;
-using AssertExtensions;
 using MechJebLib.Functions;
 using MechJebLib.Primitives;
 using Xunit;

MechJebLibTest/ManeuversTests/TwoImpulseTransferTests.cs

@@ -1,5 +1,4 @@
 using System;
-using AssertExtensions;
 using MechJebLib.Functions;
 using MechJebLib.Maneuvers;
 using MechJebLib.Primitives;

MechJebLibTest/MechJebLibTest.csproj

@@ -55,31 +55,31 @@
     <ItemGroup>
         <Compile Include="AssertionExtensions.cs"/>
         <Compile Include="ControlTests\PIDLoopTests.cs"/>
+        <Compile Include="FunctionsTests.cs" />
+        <Compile Include="LambertTests\GoodingTests.cs" />
         <Compile Include="ManeuversTests\ChangeOrbitalElementTests.cs"/>
         <Compile Include="ManeuversTests\TwoImpulseTransferTests.cs"/>
         <Compile Include="ManeuversTests\ReturnFromMoonTests.cs"/>
         <Compile Include="ManeuversTests\Simple.cs"/>
-        <Compile Include="MathsTests\BisectionTests.cs"/>
-        <Compile Include="MathsTests\BS3Tests.c.cs"/>
-        <Compile Include="MathsTests\DP5Tests.cs"/>
-        <Compile Include="MathsTests\FunctionsTests.cs"/>
-        <Compile Include="MathsTests\BrentRootTests.cs"/>
-        <Compile Include="MathsTests\GoodingTests.cs"/>
-        <Compile Include="MathsTests\NewtonTests.cs"/>
-        <Compile Include="MathsTests\TwoBody\FarnocchiaTests.cs"/>
-        <Compile Include="MathsTests\TwoBody\ShepperdTests.cs"/>
+        <Compile Include="ODETests\BS3Tests.c.cs" />
+        <Compile Include="ODETests\DP5Tests.cs" />
         <Compile Include="Properties\AssemblyInfo.cs"/>
         <Compile Include="PVGTests\AscentTests\BuggyTests.cs"/>
         <Compile Include="PVGTests\AscentTests\TheStandardTests.cs"/>
         <Compile Include="PVGTests\AscentTests\Titan2Tests.cs"/>
         <Compile Include="PVGTests\Integrators\VacuumCoastAnalyticTests.cs"/>
         <Compile Include="PVGTests\Integrators\VacuumThrustIntegratorTests.cs"/>
+        <Compile Include="RootfindingTests\BisectionTests.cs" />
+        <Compile Include="RootfindingTests\BrentRootTests.cs" />
+        <Compile Include="RootfindingTests\NewtonTests.cs" />
         <Compile Include="StaticTests.cs"/>
         <Compile Include="Structs\HTests.cs"/>
         <Compile Include="Structs\M3Tests.cs"/>
         <Compile Include="Structs\Q3Tests.cs"/>
         <Compile Include="Structs\V3Tests.cs"/>
         <Compile Include="TestInitialization.cs"/>
+        <Compile Include="TwoBodyTests\FarnocchiaTests.cs" />
+        <Compile Include="TwoBodyTests\ShepperdTests.cs" />
     </ItemGroup>
     <ItemGroup>
         <ProjectReference Include="..\MechJebLib\MechJebLib.csproj">

MechJebLibTest/MathsTests/BS3Tests.c.cs → MechJebLibTest/ODETests/BS3Tests.c.cs

@@ -5,13 +5,12 @@
 
 using System;
 using System.Collections.Generic;
-using AssertExtensions;
 using MechJebLib.ODE;
 using MechJebLib.Primitives;
 using Xunit;
 using static System.Math;
 
-namespace MechJebLibTest.MathsTests
+namespace MechJebLibTest.ODETests
 {
     public class BS3Tests
     {

MechJebLibTest/MathsTests/DP5Tests.cs → MechJebLibTest/ODETests/DP5Tests.cs

@@ -5,15 +5,14 @@
 
 using System;
 using System.Collections.Generic;
-using AssertExtensions;
 using MechJebLib.Functions;
 using MechJebLib.ODE;
 using MechJebLib.Primitives;
 using Xunit;
 using static MechJebLib.Utils.Statics;
 using static System.Math;
 
-namespace MechJebLibTest.MathsTests
+namespace MechJebLibTest.ODETests
 {
     public class DP5Tests
     {