Repository: jorgepiloto/lamberthub (language: python)

Issue:
Implement vallado2013
💻 **Solver request: vallado2013**

This algorithm appeared for the first time in Vallado's book titled "Fundamentals of astrodynamics and applications". It is an improved version of the one developed by Bate in his "Fundamentals of astrodynamics", in 1971.

📝 **References**

```
@book{vallado2013fundamentals,
  title={Fundamentals of astrodynamics and applications},
  author={Vallado, David A},
  volume={12},
  year={2013},
  publisher={Springer Science \& Business Media}
}
```


Discussion / hints:
This algorithm is already implemented in the [poliastro library](https://www.poliastro.space/). After tweaking it locally, all tests passed so let's add this to the library 🙌🏽 

Fix patch:
diff --git a/README.md b/README.md
index cf0a11e..3ab8fbd 100644
--- a/README.md
+++ b/README.md
@@ -37,7 +37,7 @@ print([solver.__name__ for solver in ALL_SOLVERS])
 At this moment, the following algorithms are available:
 
 ```bash
->>> ['gauss1809', 'battin1984', 'gooding1990', 'avanzini2008', 'arora2013', 'izzo2015']
+>>> ['gauss1809', 'battin1984', 'gooding1990', 'avanzini2008', 'arora2013', 'vallado2013', 'izzo2015']
 ```
 
 ## How can I use a solver?
diff --git a/src/lamberthub/__init__.py b/src/lamberthub/__init__.py
index 64690be..0e41244 100644
--- a/src/lamberthub/__init__.py
+++ b/src/lamberthub/__init__.py
@@ -6,10 +6,19 @@
 from lamberthub.universal_solvers.arora import arora2013
 from lamberthub.universal_solvers.gooding import gooding1990
 from lamberthub.universal_solvers.izzo import izzo2015
+from lamberthub.universal_solvers.vallado import vallado2013
 
 __version__ = "0.2.dev0"
 
-ALL_SOLVERS = [gauss1809, battin1984, gooding1990, avanzini2008, arora2013, izzo2015]
+ALL_SOLVERS = [
+    gauss1809,
+    battin1984,
+    gooding1990,
+    avanzini2008,
+    arora2013,
+    vallado2013,
+    izzo2015,
+]
 """ A list holding all lamberthub available solvers """
 
 ZERO_REV_SOLVERS = [battin1984, gooding1990, avanzini2008, arora2013, izzo2015]
diff --git a/src/lamberthub/universal_solvers/vallado.py b/src/lamberthub/universal_solvers/vallado.py
new file mode 100644
index 0000000..72f480e
--- /dev/null
+++ b/src/lamberthub/universal_solvers/vallado.py
@@ -0,0 +1,146 @@
+"""This module holds all methods devised by David A. Vallado."""
+
+import time
+
+import numpy as np
+
+from lamberthub.utils.angles import get_transfer_angle
+from lamberthub.utils.assertions import assert_parameters_are_valid
+from lamberthub.utils.stumpff import c2, c3
+
+
+def vallado2013(
+    mu,
+    r1,
+    r2,
+    tof,
+    M=0,
+    prograde=True,
+    low_path=True,
+    maxiter=100,
+    atol=1e-5,
+    rtol=1e-7,
+    full_output=False,
+):
+    r"""
+    Vallado's algorithm makes use of the universal formulation to solve for the
+    Lambert's problem. By making use of a bisection method, it guarantees the
+    convergence to the solution but the amount of iterations require
+    dramatically increases.
+
+    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 and time per iteration are also returned.
+
+    Returns
+    -------
+    v1: numpy.array
+        Initial velocity vector.
+    v2: numpy.array
+        Final velocity vector.
+    numiter: int
+        Number of iterations.
+    tpi: float
+        Time per iteration in seconds.
+
+    Notes
+    -----
+    This algorithm is presented as an alternative to the one developed by Bate
+    in 1971. Bate did not impose a particular numerical solver for his algorithm
+    but cited both bisection and Newton's one. However, for some values of the
+    boundary problem, the initial guess might diverge if Newton's solver is
+    used. That's why Vallado decided to employ a bisection method instead.
+    Although detrimental from the point of view of performance, this algorithm
+    properly reaches solution in the majority of the cases.
+
+    References
+    ----------
+    [1] Vallado, D. A. (2001). Fundamentals of astrodynamics and applications
+    (Vol. 12). Springer Science & Business Media.
+
+    """
+
+    # Check that input parameters are safe
+    assert_parameters_are_valid(mu, r1, r2, tof, M)
+
+    # Retrieve the fundamental geometry of the problem
+    r1_norm, r2_norm, c_norm = [np.linalg.norm(vec) for vec in [r1, r2, r2 - r1]]
+    dtheta = get_transfer_angle(r1, r2, prograde)
+
+    t_m = 1 if dtheta < np.pi else -1
+
+    norm_r1_times_norm_r2 = r1_norm * r2_norm
+    norm_r1_plus_norm_r2 = r1_norm + r2_norm
+
+    cos_dnu = np.dot(r1, r2) / norm_r1_times_norm_r2
+
+    A = t_m * (r1_norm * r2_norm * (1 + cos_dnu)) ** 0.5
+
+    if A == 0.0:
+        raise RuntimeError("Cannot compute orbit, phase angle is 180 degrees")
+
+    psi = 0.0
+    psi_low = -4 * np.pi ** 2
+    psi_up = 4 * np.pi ** 2
+
+    tic = time.perf_counter()
+    for numiter in range(1, maxiter + 1):
+        y = norm_r1_plus_norm_r2 + A * (psi * c3(psi) - 1) / c2(psi) ** 0.5
+        if A > 0.0:
+            # Readjust xi_low until y > 0.0
+            # Translated directly from Vallado
+            while y < 0.0:
+                psi_low = psi
+                psi = (
+                    0.8
+                    * (1.0 / c3(psi))
+                    * (1.0 - norm_r1_times_norm_r2 * np.sqrt(c2(psi)) / A)
+                )
+                y = norm_r1_plus_norm_r2 + A * (psi * c3(psi) - 1) / c2(psi) ** 0.5
+
+        xi = np.sqrt(y / c2(psi))
+        tof_new = (xi ** 3 * c3(psi) + A * np.sqrt(y)) / np.sqrt(mu)
+
+        # Convergence check
+        if np.abs((tof_new - tof) / tof) < rtol:
+            tac = time.perf_counter()
+            tpi = (tac - tic) / numiter
+            break
+
+        # Bisection check
+        condition = tof_new <= tof
+        psi_low = psi_low + (psi - psi_low) * condition
+        psi_up = psi_up + (psi - psi_up) * (not condition)
+
+        psi = (psi_up + psi_low) / 2
+    else:
+        raise ValueError("Exceeded maximum number of iterations!")
+
+    f = 1 - y / r1_norm
+    g = A * np.sqrt(y / mu)
+
+    gdot = 1 - y / r2_norm
+
+    v1 = (r2 - f * r1) / g
+    v2 = (gdot * r2 - r1) / g
+
+    return (v1, v2, numiter, tpi) if full_output is True else (v1, v2)
diff --git a/src/lamberthub/utils/stumpff.py b/src/lamberthub/utils/stumpff.py
new file mode 100644
index 0000000..a9757f7
--- /dev/null
+++ b/src/lamberthub/utils/stumpff.py
@@ -0,0 +1,62 @@
+"""
+This module holds the so-called Stumpff functions. These set of functions
+usually appear in a plethora of equations within the subject of astrodynamics.
+"""
+
+from math import gamma
+
+import numpy as np
+
+
+def c2(psi):
+    r"""Second Stumpff function.
+
+    For positive arguments:
+
+    .. math::
+
+        c_2(\psi) = \frac{1 - \cos{\sqrt{\psi}}}{\psi}
+
+    """
+    eps = 1.0
+    if psi > eps:
+        res = (1 - np.cos(np.sqrt(psi))) / psi
+    elif psi < -eps:
+        res = (np.cosh(np.sqrt(-psi)) - 1) / (-psi)
+    else:
+        res = 1.0 / 2.0
+        delta = (-psi) / gamma(2 + 2 + 1)
+        k = 1
+        while res + delta != res:
+            res = res + delta
+            k += 1
+            delta = (-psi) ** k / gamma(2 * k + 2 + 1)
+
+    return res
+
+
+def c3(psi):
+    r"""Third Stumpff function.
+
+    For positive arguments:
+
+    .. math::
+
+        c_3(\psi) = \frac{\sqrt{\psi} - \sin{\sqrt{\psi}}}{\sqrt{\psi^3}}
+
+    """
+    eps = 1.0
+    if psi > eps:
+        res = (np.sqrt(psi) - np.sin(np.sqrt(psi))) / (psi * np.sqrt(psi))
+    elif psi < -eps:
+        res = (np.sinh(np.sqrt(-psi)) - np.sqrt(-psi)) / (-psi * np.sqrt(-psi))
+    else:
+        res = 1.0 / 6.0
+        delta = (-psi) / gamma(2 + 3 + 1)
+        k = 1
+        while res + delta != res:
+            res = res + delta
+            k += 1
+            delta = (-psi) ** k / gamma(2 * k + 3 + 1)
+
+    return res


Test patch:
diff --git a/tests/tests_universal_solvers/test_vallado.py b/tests/tests_universal_solvers/test_vallado.py
new file mode 100644
index 0000000..3251e1d
--- /dev/null
+++ b/tests/tests_universal_solvers/test_vallado.py
@@ -0,0 +1,34 @@
+import numpy as np
+import pytest
+
+from lamberthub import vallado2013
+
+
+def test_exception_for_180_transfer_angle():
+    # Initial conditions
+    mu_earth = 3.986004418e5  # [km ** 3 / s ** 2]
+    r1 = np.array([1.0, 0.0, 0.0])  # [km]
+    r2 = np.array([-1.0, 0.0, 0.0])  # [km]
+    tof = 1000  # [s]
+
+    # Solving the problem with only two iteration so an error is raised
+    with pytest.raises(RuntimeError) as excinfo:
+        v1, v2 = vallado2013(
+            mu_earth, r1, r2, tof, maxiter=1, prograde=True, low_path=True
+        )
+    assert "Cannot compute orbit, phase angle is 180 degrees" in excinfo.exconly()
+
+
+def test_raised_maximum_number_of_iterations():
+    # Initial conditions
+    mu_earth = 3.986004418e5  # [km ** 3 / s ** 2]
+    r1 = np.array([1.0, 0.0, 0.0])  # [km]
+    r2 = np.array([0.0, 1.0, 0.0])  # [km]
+    tof = 1000  # [s]
+
+    # Solving the problem with only two iteration so an error is raised
+    with pytest.raises(ValueError) as excinfo:
+        v1, v2 = vallado2013(
+            mu_earth, r1, r2, tof, maxiter=1, prograde=True, low_path=True
+        )
+    assert "Exceeded maximum number of iterations!" in excinfo.exconly()