Repository: jorgepiloto/lamberthub (language: python)

Issue:
Implement izzo2015 solver
💻 **Solver request: izzo2015**

Being one of the most popular modern solvers, this algorithm is used by many astrodynamics and orbital mechanics softwares. It is known to be complete, fast and accurate. It exploits again the universal formulae, same as Lancaster, but introduces an additional change of variable which leads to smoother time of flight curves. This translates into fast convergence when making the use of a Householder root-finder.

Izzo published its algorithm in 2015 and provided [the source code](https://github.com/esa/pykep/blob/master/src/lambert_problem.cpp). However, Izzo's software is released under GPL license but here it comes [poliastro](https://github.com/poliastro/poliastro/) to the rescue! This library provides [the Python implementation](https://github.com/poliastro/poliastro/blob/main/src/poliastro/core/iod.py) under MIT license, which is compatible with the current APACHE-2.0 one. Some logic might be needed to be reformatted in order to fulfill `lamberthub` implementation requirements in terms of input and output parameters.

📝 **References**

All the BibTeX materials:

```@article{izzo2015revisiting,
  title={Revisiting Lambert’s problem},
  author={Izzo, Dario},
  journal={Celestial Mechanics and Dynamical Astronomy},
  volume={121},
  number={1},
  pages={1--15},
  year={2015},
  publisher={Springer}
}
```



Discussion / hints:

Fix patch:
diff --git a/README.md b/README.md
index d9c0661..b101e1e 100644
--- a/README.md
+++ b/README.md
@@ -33,7 +33,7 @@ print([solver.__name__ for solver in ALL_SOLVERS])
 At this moment, the following algorithms are available:
 
 ```bash
->>> ['gooding1990']
+>>> ['gooding1990', 'izzo2015']
 ```
 
 ## How can I use a solver?
diff --git a/src/lamberthub/__init__.py b/src/lamberthub/__init__.py
index ecb8772..f182560 100644
--- a/src/lamberthub/__init__.py
+++ b/src/lamberthub/__init__.py
@@ -1,17 +1,18 @@
 """ A collection of Lambert's problem solvers """
 
 from lamberthub.universal_solvers.gooding import gooding1990
+from lamberthub.universal_solvers.izzo import izzo2015
 
 __version__ = "0.1.dev0"
 
-ALL_SOLVERS = [gooding1990]
+ALL_SOLVERS = [gooding1990, izzo2015]
 """ A list holding all lamberthub available solvers """
 
-ZERO_REV_SOLVERS = [gooding1990]
+ZERO_REV_SOLVERS = [gooding1990, izzo2015]
 """ A list holding all direct-revolution lamberthub solvers """
 
-MULTI_REV_SOLVERS = [gooding1990]
+MULTI_REV_SOLVERS = [gooding1990, izzo2015]
 """ A list holding all multi-revolution lamberthub solvers """
 
-ROBUST_SOLVERS = [gooding1990]
+ROBUST_SOLVERS = [gooding1990, izzo2015]
 """ A list holding all robust lamberthub solvers """
diff --git a/src/lamberthub/universal_solvers/izzo.py b/src/lamberthub/universal_solvers/izzo.py
new file mode 100644
index 0000000..ae4bcd8
--- /dev/null
+++ b/src/lamberthub/universal_solvers/izzo.py
@@ -0,0 +1,341 @@
+""" A module hosting all algorithms devised by Izzo """
+
+import numpy as np
+from numpy import cross, pi
+from numpy.linalg import norm
+from scipy.special import hyp2f1
+
+from lamberthub.utils.assertions import assert_parameters_are_valid
+
+
+def izzo2015(
+    mu,
+    r1,
+    r2,
+    tof,
+    M=0,
+    prograde=True,
+    low_path=True,
+    maxiter=35,
+    atol=1e-5,
+    rtol=1e-7,
+    full_output=False,
+):
+    r"""
+    Solves Lambert problem using Izzo's devised algorithm.
+
+    Parameters
+    ----------
+    mu: float
+        Gravitational parameter, equivalent to :math:`GM` of attractor body.
+    r1: numpy.array
+        Initial position vector.
+    r2: numpy.array
+        Final position vector.
+    M: int
+        Number of revolutions. Must be equal or greater than 0 value.
+    prograde: bool
+        If `True`, specifies prograde motion. Otherwise, retrograde motion is imposed.
+    low_path: bool
+        If two solutions are available, it selects between high or low path.
+    maxiter: int
+        Maximum number of iterations.
+    atol: float
+        Absolute tolerance.
+    rtol: float
+        Relative tolerance.
+    full_output: bool
+        If True, the number of iterations is also returned.
+
+    Returns
+    -------
+    v1: numpy.array
+        Initial velocity vector.
+    v2: numpy.array
+        Final velocity vector.
+    numiter: list
+        Number of iterations.
+
+    Notes
+    -----
+    This is the algorithm devised by Dario Izzo[1] in 2015. It inherits from
+    the one developed by Lancaster[2] during the 60s, following the universal
+    formulae approach. It is one of the most modern solvers, being a complete
+    Lambert's problem solver (zero and Multiple-revolution solutions). It shows
+    high performance and robustness while requiring no more than four iterations
+    to reach a solution.
+
+    All credits of the implementation go to Juan Luis Cano Rodríguez and the
+    poliastro development team, from which this routine inherits. Some changes
+    were made to adapt it to `lamberthub` API. In addition, the hypergeometric
+    function is the one from SciPy.
+
+    Copyright (c) 2012-2021 Juan Luis Cano Rodríguez and the poliastro development team
+
+    References
+    ----------
+    [1] Izzo, D. (2015). Revisiting Lambert’s problem. Celestial Mechanics
+           and Dynamical Astronomy, 121(1), 1-15.
+
+    [2] Lancaster, E. R., & Blanchard, R. C. (1969). A unified form of
+           Lambert's theorem (Vol. 5368). National Aeronautics and Space
+           Administration.
+
+    """
+
+    # Check that input parameters are safe
+    assert_parameters_are_valid(mu, r1, r2, tof, M)
+
+    # Chord
+    c = r2 - r1
+    c_norm, r1_norm, r2_norm = norm(c), norm(r1), norm(r2)
+
+    # Semiperimeter
+    s = (r1_norm + r2_norm + c_norm) * 0.5
+
+    # Versors
+    i_r1, i_r2 = r1 / r1_norm, r2 / r2_norm
+    i_h = cross(i_r1, i_r2)
+    i_h = i_h / norm(i_h)
+
+    # Geometry of the problem
+    ll = np.sqrt(1 - min(1.0, c_norm / s))
+
+    # Compute the fundamental tangential directions
+    if i_h[2] < 0:
+        ll = -ll
+        i_t1, i_t2 = cross(i_r1, i_h), cross(i_r2, i_h)
+    else:
+        i_t1, i_t2 = cross(i_h, i_r1), cross(i_h, i_r2)
+
+    # Correct transfer angle parameter and tangential vectors regarding orbit's
+    # inclination
+    ll, i_t1, i_t2 = (-ll, -i_t1, -i_t2) if prograde is False else (ll, i_t1, i_t2)
+
+    # Non dimensional time of flight
+    T = np.sqrt(2 * mu / s ** 3) * tof
+
+    # Find solutions and filter them
+    x, y, numiter = _find_xy(ll, T, M, maxiter, rtol, low_path)
+
+    # Reconstruct
+    gamma = np.sqrt(mu * s / 2)
+    rho = (r1_norm - r2_norm) / c_norm
+    sigma = np.sqrt(1 - rho ** 2)
+
+    # Compute the radial and tangential components at initial and final
+    # position vectors
+    V_r1, V_r2, V_t1, V_t2 = _reconstruct(x, y, r1_norm, r2_norm, ll, gamma, rho, sigma)
+
+    # Solve for the initial and final velocity
+    v1 = V_r1 * (r1 / r1_norm) + V_t1 * i_t1
+    v2 = V_r2 * (r2 / r2_norm) + V_t2 * i_t2
+
+    return (v1, v2, numiter) if full_output is True else (v1, v2)
+
+
+def _reconstruct(x, y, r1, r2, ll, gamma, rho, sigma):
+    """Reconstruct solution velocity vectors."""
+    V_r1 = gamma * ((ll * y - x) - rho * (ll * y + x)) / r1
+    V_r2 = -gamma * ((ll * y - x) + rho * (ll * y + x)) / r2
+    V_t1 = gamma * sigma * (y + ll * x) / r1
+    V_t2 = gamma * sigma * (y + ll * x) / r2
+    return [V_r1, V_r2, V_t1, V_t2]
+
+
+def _find_xy(ll, T, M, maxiter, rtol, low_path):
+    """Computes all x, y for given number of revolutions."""
+    # For abs(ll) == 1 the derivative is not continuous
+    assert abs(ll) < 1
+
+    M_max = np.floor(T / pi)
+    T_00 = np.arccos(ll) + ll * np.sqrt(1 - ll ** 2)  # T_xM
+
+    # Refine maximum number of revolutions if necessary
+    if T < T_00 + M_max * pi and M_max > 0:
+        _, T_min = _compute_T_min(ll, M_max, maxiter, rtol)
+        if T < T_min:
+            M_max -= 1
+
+    # Check if a feasible solution exist for the given number of revolutions
+    # This departs from the original paper in that we do not compute all solutions
+    if M > M_max:
+        raise ValueError("No feasible solution, try lower M!")
+
+    # Initial guess
+    x_0 = _initial_guess(T, ll, M, low_path)
+
+    # Start Householder iterations from x_0 and find x, y
+    x, numiter = _householder(x_0, T, ll, M, rtol, maxiter)
+    y = _compute_y(x, ll)
+
+    return x, y, numiter
+
+
+def _compute_y(x, ll):
+    """Computes y."""
+    return np.sqrt(1 - ll ** 2 * (1 - x ** 2))
+
+
+def _compute_psi(x, y, ll):
+    """Computes psi.
+
+    "The auxiliary angle psi is computed using Eq.(17) by the appropriate
+    inverse function"
+
+    """
+    if -1 <= x < 1:
+        # Elliptic motion
+        # Use arc cosine to avoid numerical errors
+        return np.arccos(x * y + ll * (1 - x ** 2))
+    elif x > 1:
+        # Hyperbolic motion
+        # The hyperbolic sine is bijective
+        return np.arcsinh((y - x * ll) * np.sqrt(x ** 2 - 1))
+    else:
+        # Parabolic motion
+        return 0.0
+
+
+def _tof_equation(x, T0, ll, M):
+    """Time of flight equation."""
+    return _tof_equation_y(x, _compute_y(x, ll), T0, ll, M)
+
+
+def _tof_equation_y(x, y, T0, ll, M):
+    """Time of flight equation with externally computated y."""
+    if M == 0 and np.sqrt(0.6) < x < np.sqrt(1.4):
+        eta = y - ll * x
+        S_1 = (1 - ll - x * eta) * 0.5
+        Q = 4 / 3 * hyp2f1(3, 1, 5 / 2, S_1)
+        T_ = (eta ** 3 * Q + 4 * ll * eta) * 0.5
+    else:
+        psi = _compute_psi(x, y, ll)
+        T_ = np.divide(
+            np.divide(psi + M * pi, np.sqrt(np.abs(1 - x ** 2))) - x + ll * y,
+            (1 - x ** 2),
+        )
+
+    return T_ - T0
+
+
+def _tof_equation_p(x, y, T, ll):
+    # TODO: What about derivatives when x approaches 1?
+    return (3 * T * x - 2 + 2 * ll ** 3 * x / y) / (1 - x ** 2)
+
+
+def _tof_equation_p2(x, y, T, dT, ll):
+    return (3 * T + 5 * x * dT + 2 * (1 - ll ** 2) * ll ** 3 / y ** 3) / (1 - x ** 2)
+
+
+def _tof_equation_p3(x, y, _, dT, ddT, ll):
+    return (7 * x * ddT + 8 * dT - 6 * (1 - ll ** 2) * ll ** 5 * x / y ** 5) / (
+        1 - x ** 2
+    )
+
+
+def _compute_T_min(ll, M, maxiter, rtol):
+    """Compute minimum T."""
+    if ll == 1:
+        x_T_min = 0.0
+        T_min = _tof_equation(x_T_min, 0.0, ll, M)
+    else:
+        if M == 0:
+            x_T_min = np.inf
+            T_min = 0.0
+        else:
+            # Set x_i > 0 to avoid problems at ll = -1
+            x_i = 0.1
+            T_i = _tof_equation(x_i, 0.0, ll, M)
+            x_T_min = _halley(x_i, T_i, ll, rtol, maxiter)
+            T_min = _tof_equation(x_T_min, 0.0, ll, M)
+
+    return [x_T_min, T_min]
+
+
+def _initial_guess(T, ll, M, low_path):
+    """Initial guess."""
+    if M == 0:
+        # Single revolution
+        T_0 = np.arccos(ll) + ll * np.sqrt(1 - ll ** 2) + M * pi  # Equation 19
+        T_1 = 2 * (1 - ll ** 3) / 3  # Equation 21
+        if T >= T_0:
+            x_0 = (T_0 / T) ** (2 / 3) - 1
+        elif T < T_1:
+            x_0 = 5 / 2 * T_1 / T * (T_1 - T) / (1 - ll ** 5) + 1
+        else:
+            # This is the real condition, which is not exactly equivalent
+            # elif T_1 < T < T_0
+            x_0 = (T_0 / T) ** (np.log2(T_1 / T_0)) - 1
+
+        return x_0
+    else:
+        # Multiple revolution
+        x_0l = (((M * pi + pi) / (8 * T)) ** (2 / 3) - 1) / (
+            ((M * pi + pi) / (8 * T)) ** (2 / 3) + 1
+        )
+        x_0r = (((8 * T) / (M * pi)) ** (2 / 3) - 1) / (
+            ((8 * T) / (M * pi)) ** (2 / 3) + 1
+        )
+
+        # Filter out the solution
+        x_0 = np.max([x_0l, x_0r]) if low_path is True else np.min([x_0l, x_0r])
+
+        return x_0
+
+
+def _halley(p0, T0, ll, tol, maxiter):
+    """Find a minimum of time of flight equation using the Halley method.
+
+    Note
+    ----
+    This function is private because it assumes a calling convention specific to
+    this module and is not really reusable.
+
+    """
+    for ii in range(1, maxiter + 1):
+        y = _compute_y(p0, ll)
+        fder = _tof_equation_p(p0, y, T0, ll)
+        fder2 = _tof_equation_p2(p0, y, T0, fder, ll)
+        if fder2 == 0:
+            raise RuntimeError("Derivative was zero")
+        fder3 = _tof_equation_p3(p0, y, T0, fder, fder2, ll)
+
+        # Halley step (cubic)
+        p = p0 - 2 * fder * fder2 / (2 * fder2 ** 2 - fder * fder3)
+
+        if abs(p - p0) < tol:
+            return p
+        p0 = p
+
+    raise RuntimeError("Failed to converge")
+
+
+def _householder(p0, T0, ll, M, tol, maxiter):
+    """Find a zero of time of flight equation using the Householder method.
+
+    Note
+    ----
+    This function is private because it assumes a calling convention specific to
+    this module and is not really reusable.
+
+    """
+    for numiter in range(1, maxiter + 1):
+        y = _compute_y(p0, ll)
+        fval = _tof_equation_y(p0, y, T0, ll, M)
+        T = fval + T0
+        fder = _tof_equation_p(p0, y, T, ll)
+        fder2 = _tof_equation_p2(p0, y, T, fder, ll)
+        fder3 = _tof_equation_p3(p0, y, T, fder, fder2, ll)
+
+        # Householder step (quartic)
+        p = p0 - fval * (
+            (fder ** 2 - fval * fder2 / 2)
+            / (fder * (fder ** 2 - fval * fder2) + fder3 * fval ** 2 / 6)
+        )
+
+        if abs(p - p0) < tol:
+            return p, numiter
+        p0 = p
+
+    raise RuntimeError("Failed to converge")


Test patch:
diff --git a/tests/test_all_solvers.py b/tests/test_all_solvers.py
index 7c7e258..2f7ab88 100644
--- a/tests/test_all_solvers.py
+++ b/tests/test_all_solvers.py
@@ -7,14 +7,6 @@
 from lamberthub import ALL_SOLVERS
 
 
-@pytest.mark.parametrize("solver", ALL_SOLVERS)
-def test_transfer_angle_is_zero_raises_exception(solver):
-    with pytest.raises(ValueError) as excinfo:
-        r1, r2 = [i * np.ones(3) for i in range(1, 3)]
-        solver(1.00, r1, r2, 1.00)
-    assert "Transfer angle was found to be zero!" in excinfo.exconly()
-
-
 @pytest.mark.parametrize("solver", ALL_SOLVERS)
 def test_case_from_vallado_book(solver):
     """
diff --git a/tests/tests_universal_solvers/test_gooding.py b/tests/tests_universal_solvers/test_gooding.py
new file mode 100644
index 0000000..6c8c31c
--- /dev/null
+++ b/tests/tests_universal_solvers/test_gooding.py
@@ -0,0 +1,11 @@
+import numpy as np
+import pytest
+
+from lamberthub import gooding1990
+
+
+def test_transfer_angle_is_zero_raises_exception():
+    with pytest.raises(ValueError) as excinfo:
+        r1, r2 = [i * np.ones(3) for i in range(1, 3)]
+        gooding1990(1.00, r1, r2, 1.00)
+    assert "Transfer angle was found to be zero!" in excinfo.exconly()
diff --git a/tests/tests_universal_solvers/test_izzo.py b/tests/tests_universal_solvers/test_izzo.py
new file mode 100644
index 0000000..3a1d6e1
--- /dev/null
+++ b/tests/tests_universal_solvers/test_izzo.py
@@ -0,0 +1,12 @@
+import pytest
+
+from lamberthub.universal_solvers import izzo
+
+
+@pytest.mark.parametrize("M", [1, 2, 3])
+def test_minimum_time_of_flight_convergence(M):
+    ll = -1
+    x_T_min_expected, T_min_expected = izzo._compute_T_min(ll, M, maxiter=10, rtol=1e-8)
+    y = izzo._compute_y(x_T_min_expected, ll)
+    T_min = izzo._tof_equation_y(x_T_min_expected, y, 0.0, ll, M)
+    assert T_min_expected == T_min