Adding Izzo's Lambert method

This isn't actually any faster than Gooding in the nrev=0 case, so I
don't think we should use it over Gooding right now.

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

MechJeb2/Maneuver/TransferCalculator.cs

@@ -6,6 +6,7 @@
 using System.Reflection;
 using System.Threading;
 using MechJebLib.Core;
+using MechJebLib.Core.Lambert;
 using MechJebLib.Primitives;
 using UnityEngine;
 using UnityToolbag;

MechJeb2/MechJebLib/Core/Gooding.cs → MechJeb2/MechJebLib/Core/Lambert/Gooding.cs

@@ -4,6 +4,7 @@
  */
 
 using System;
+using System.Runtime.CompilerServices;
 using MechJebLib.Primitives;
 using MechJebLib.Utils;
 using static MechJebLib.Statics;
@@ -14,7 +15,7 @@
 
 #nullable enable
 
-namespace MechJebLib.Core
+namespace MechJebLib.Core.Lambert
 {
     public static class Gooding
     {
@@ -122,6 +123,7 @@ public static (V3 Vi, V3 Vf) Solve(double mu, V3 r1, V3 v1, V3 r2, double tof, i
          * kept as super-fugly looking C# code for those reasons.
          */
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
         internal static ( int N, double VR11, double VT11, double VR12, double VT12, double VR21, double VT21, double VR22,
             double VT22) VLAMB(double GM, double R1, double R2, double TH, double TDELT)
         {
@@ -196,6 +198,7 @@ public static (V3 Vi, V3 Vf) Solve(double mu, V3 r1, V3 v1, V3 r2, double tof, i
             return (N, VR11, VT11, VR12, VT12, VR21, VT21, VR22, VT22);
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
         private static ( int N, double X, double XPL) XLAMB(int M, double Q, double QSQFM1, double TIN)
         {
             Check.Finite(M);
@@ -349,6 +352,7 @@ private static ( int N, double X, double XPL) XLAMB(int M, double Q, double QSQF
             goto Five;
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
         private static ( double T, double DT, double D2T, double D3T) TLAMB(int M, double Q, double QSQFM1, double X, int N)
         {
             Check.Finite(X);

MechJeb2/MechJebLib/Core/Lambert/Izzo.cs

@@ -0,0 +1,319 @@
+/*
+ * 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+
+ */
+
+using System;
+using System.Runtime.CompilerServices;
+using MechJebLib.Primitives;
+using static System.Math;
+using static MechJebLib.Statics;
+
+#nullable enable
+
+namespace MechJebLib.Core.Lambert
+{
+    /// <summary>
+    ///     This is Izzo's method, copied from:
+    ///     https://github.com/esa/pykep/blob/8b0e9444d09b909d7d1d11e951c8efcfde0a2ffd/src/lambert_problem.cpp
+    ///     I made some changes to the API, largely around making it conform to the Gooding method API that we had
+    ///     previously and not using the clockwise/counterclockwise API so it doesn't have that weird singularity
+    ///     for perfectly polar orbits.
+    ///     This implementation isn't any faster than the Gooding implementation in this repo for nrev=0.
+    /// </summary>
+    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);
+
+            if (_nsol == 0)
+                return (_v1[0], _v2[0]);
+
+            double mindv = double.PositiveInfinity;
+            int index = 0;
+
+            for (int i = 0; i < _nsol; i++)
+            {
+                double dv = (_v1[i] - v1).sqrMagnitude;
+                if (dv >= mindv)
+                    continue;
+
+                index = i;
+                mindv = dv;
+            }
+
+            return (_v1[index], _v2[index]);
+        }
+
+        private int      _nsol;
+        private V3[]     _v1    = new V3[5];
+        private V3[]     _v2    = new V3[5];
+        private int[]    _iters = new int[5];
+        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)
+        {
+            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);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private void lambert_problem(V3 r1, V3 r2, double tof, double mu, 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!");
+
+            // 1 - Getting lambda and T
+            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);
+
+            var it1 = V3.Cross(ih, ir1);
+            var it2 = V3.Cross(ih, ir2);
+
+            it1 = it1.normalized;
+            it2 = it2.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];
+            }
+
+            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
+            int nmax = (int)(t / PI);
+            double t00 = Acos(lambda) + lambda * Sqrt(1.0 - lambda2);
+            double t0 = t00 + nmax * PI;
+            double t1 = 2.0 / 3.0 * (1.0 - lambda3);
+            if (nmax > 0)
+            {
+                if (t < t0)
+                {
+                    // We use Halley iterations to find xM and TM
+                    int it = 0;
+                    double tMin = t0;
+                    double xOld = 0.0, xNew = 0.0;
+                    while (true)
+                    {
+                        (double dt, double ddt, double dddt) = DTdx(lambda, xOld, tMin);
+                        if (dt != 0.0)
+                            xNew = xOld - dt * ddt / (ddt * ddt - dt * dddt / 2.0);
+
+                        double err = Abs(xOld - xNew);
+                        if (err < 1e-13 || it > 12)
+                            break;
+
+                        tMin = X2Tof(lambda, xNew, nmax);
+                        xOld = xNew;
+                        it++;
+                    }
+
+                    if (tMin > t)
+                        nmax -= 1;
+                }
+            }
+
+            // 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
+            nmax  = Min(nrev, nmax);
+            _nsol = nmax * 2 + 1;
+
+            // 2.2 We now allocate the memory for the output variables
+            if (_nsol > _x.Length)
+            {
+                _v1    = new V3[nmax * 2 + 1];
+                _v2    = new V3[nmax * 2 + 1];
+                _iters = new int[nmax * 2 + 1];
+                _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
+            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
+            _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
+                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
+                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
+            double gamma = Sqrt(mu * s / 2.0);
+            double rho = (r1M - r2M) / c;
+            double sigma = Sqrt(1 - rho * rho);
+            for (int i = 0; i < _nsol; ++i)
+            {
+                double y = Sqrt(1.0 - lambda2 + lambda2 * _x[i] * _x[i]);
+                double vr1 = gamma * (lambda * y - _x[i] - rho * (lambda * y + _x[i])) / r1M;
+                double vr2 = -gamma * (lambda * y - _x[i] + rho * (lambda * y + _x[i])) / r2M;
+                double vt = gamma * sigma * (y + lambda * _x[i]);
+                double vt1 = vt / r1M;
+                double vt2 = vt / r2M;
+                for (int j = 0; j < 3; ++j)
+                    _v1[i][j] = vr1 * ir1[j] + vt1 * it1[j];
+                for (int j = 0; j < 3; ++j)
+                    _v2[i][j] = vr2 * ir2[j] + vt2 * it2[j];
+            }
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static int Householder(double lambda, double t, ref double x0, int n, double eps, int maxIter)
+        {
+            int it = 0;
+            double err = 1.0;
+            while (err > eps && it < maxIter)
+            {
+                double tof = X2Tof(lambda, x0, n);
+                (double dt, double ddt, double dddt) = DTdx(lambda, x0, tof);
+                double delta = tof - t;
+                double dt2 = dt * dt;
+                double xnew = x0 - delta * (dt2 - delta * ddt / 2.0) / (dt * (dt2 - delta * ddt) + dddt * delta * delta / 6.0);
+                err = Abs(x0 - xnew);
+                x0  = xnew;
+                it++;
+            }
+
+            return it;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static ( double DT, double DDT, double DDDT) DTdx(double lambda, double x, double t)
+        {
+            double l2 = lambda * lambda;
+            double l3 = l2 * lambda;
+            double umx2 = 1.0 - x * x;
+            double y = Sqrt(1.0 - l2 * umx2);
+            double y2 = y * y;
+            double y3 = y2 * y;
+            double dt = 1.0 / umx2 * (3.0 * t * x - 2.0 + 2.0 * l3 * x / y);
+            double ddt = 1.0 / umx2 * (3.0 * t + 5.0 * x * dt + 2.0 * (1.0 - l2) * l3 / y3);
+            double dddt = 1.0 / umx2 * (7.0 * x * ddt + 8.0 * dt - 6.0 * (1.0 - l2) * l2 * l3 * x / y3 / y2);
+            return (dt, ddt, dddt);
+        }
+
+        [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)
+        {
+            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);
+
+            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
+                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
+            double y = Sqrt(rho);
+            double g = x * z - lambda * e;
+            double d;
+            if (e < 0)
+            {
+                double l = Acos(g);
+                d = n * PI + l;
+            }
+            else
+            {
+                double f = y * (z - lambda * x);
+                d = Log(f + g);
+            }
+
+            return (x - lambda * z - d / y) / e;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static double HypergeometricF(double z, double tol)
+        {
+            double sj = 1.0;
+            double cj = 1.0;
+            double err = 1.0;
+            int j = 0;
+            while (err > tol)
+            {
+                double cj1 = cj * (3.0 + j) * (1.0 + j) / (2.5 + j) * z / (j + 1);
+                double sj1 = sj + cj1;
+                err =  Abs(cj1);
+                sj  =  sj1;
+                cj  =  cj1;
+                j   += 1;
+            }
+
+            return sj;
+        }
+    }
+}

MechJeb2/MechJebLib/Maneuvers/TwoImpulseTransfer.cs

@@ -1,5 +1,6 @@
 using System;
 using MechJebLib.Core;
+using MechJebLib.Core.Lambert;
 using MechJebLib.Core.TwoBody;
 using MechJebLib.Primitives;
 using MechJebLib.Utils;

MechJeb2/OrbitalManeuverCalculator.cs

@@ -1,5 +1,6 @@
 using System;
 using MechJebLib.Core;
+using MechJebLib.Core.Lambert;
 using MechJebLib.Maneuvers;
 using MechJebLib.Primitives;
 using Smooth.Pools;