Support for Battin's Method?

[hikari20XX]

Hello!

I've been using this toolkit a lot, and wanted to contribute. Based off the code found here (https://github.com/Spacecraft-Code/Vallado/blob/master/Fortran/ASTIOD.FOR), I have an updated version of Battin's Method. There are some references to other modules within my specific project (shouldn't be crazy to replace them), but hopefully this could be useful to someone!

! Battin method
subroutine solve_battin(r1, r2, tof, theta, tau, soln)
use lambert_utils, only: wp, pi
use orbital_constants, only: mu_canonical
implicit none
real(wp), dimension(3), intent(in) :: r1, r2
real(wp), intent(in) :: tof, theta, tau
type(lambert_soln_type), intent(out) :: soln

! Local variables
real(wp) :: r1_mag, r2_mag, c, s
real(wp) :: RoR, eps, tan2w, rp
real(wp) :: l, m, x, xn, lim1, see_value, denom
real(wp) :: h1, h2, b_var, u, k_value, y_sqrt, y, dt_diff
real(wp) :: a, arg1, arg2, AlpE, BetE, DE, f, g, gdot
real(wp) :: delta_nu, chord
integer :: loops

! Initialize
soln%converged = .false.
soln%iterations = 0
soln%error = huge(1.0_wp)  ! Initialize error to a large number

! Calculate magnitudes and geometry
r1_mag = norm2_vec(r1)
r2_mag = norm2_vec(r2)
chord = norm2_vec(r2 - r1)
s = (r1_mag + r2_mag + chord) / 2.0_wp
RoR = r2_mag / r1_mag
eps = RoR - 1.0_wp
delta_nu = theta
tan2w = 0.25_wp * eps ** 2 / (sqrt(RoR) + RoR * (2.0_wp + sqrt(RoR)))
rp = sqrt(r1_mag * r2_mag) * ((cos(delta_nu / 4.0_wp)) ** 2 + tan2w)

if (delta_nu < pi) then
    l = ((sin(delta_nu / 4.0_wp)) ** 2 + tan2w) / (((sin(delta_nu / 4.0_wp)) ** 2) + tan2w + cos(delta_nu / 2.0_wp))
else
    l = (((cos(delta_nu / 4.0_wp)) ** 2) + tan2w - cos(delta_nu / 2.0_wp)) / (((cos(delta_nu / 4.0_wp)) ** 2) + tan2w)
endif

m = mu_canonical * tof ** 2 / (8.0_wp * rp ** 3)
x = 10.0_wp
xn = l
lim1 = sqrt(m / l)

loops = 0
do while ((abs(xn - x) >= TOL) .and. (loops <= MAX_ITER))
    x = xn
    see_value = see(x)
    denom = 1.0_wp / ((1.0_wp + 2.0_wp * x + l) * (4.0_wp * x + see_value * (3.0_wp + x)))
    h1 = (l + x) ** 2 * (1.0_wp + 3.0_wp * x + see_value) * denom
    h2 = m * (x - l + see_value) * denom

    ! Evaluate cubic
    b_var = 0.25_wp * 27.0_wp * h2 / ((1.0_wp + h1) ** 3)
    if (b_var < -1.0_wp) then
        xn = 1.0_wp - 2.0_wp * l
    else
        u = 0.5_wp * b_var / (1.0_wp + sqrt(1.0_wp + b_var))
        k_value = k_func(u)
        y_sqrt = sqrt(1.0_wp + b_var)
        y = ((1.0_wp + h1) / 3.0_wp) * (2.0_wp + y_sqrt / (1.0_wp + 2.0_wp * u * k_value ** 2))
        xn = sqrt(((1.0_wp - l) * 0.5_wp) ** 2 + m / (y ** 2)) - (1.0_wp + l) * 0.5_wp
    endif
    loops = loops + 1
end do

soln%iterations = loops
dt_diff = abs(xn - x)
soln%error = dt_diff  ! Set the convergence error

if (dt_diff < TOL) then
    soln%converged = .true.
    a = mu_canonical * tof ** 2 / (16.0_wp * rp ** 2 * xn * y ** 2)

    ! Determine orbit type
    if (a < -NEAR_ZERO) then
        ! Hyperbolic case
        arg1 = sqrt(s / (-2.0_wp * a))
        arg2 = sqrt((s - chord) / (-2.0_wp * a))
        AlpE = 2.0_wp * asinh(arg1)
        BetE = 2.0_wp * asinh(arg2)
        DE = AlpE - BetE
        f = 1.0_wp - (a / r1_mag) * (1.0_wp - cosh(DE))
        g = tof - sqrt(-a ** 3 / mu_canonical) * (sinh(DE) - DE)
        gdot = 1.0_wp - (a / r2_mag) * (1.0_wp - cosh(DE))
    else if (a > NEAR_ZERO) then
        ! Elliptical case
        arg1 = sqrt(s / (2.0_wp * a))
        arg2 = sqrt((s - chord) / (2.0_wp * a))
        AlpE = 2.0_wp * asin(arg1)
        BetE = 2.0_wp * asin(arg2)
        DE = AlpE - BetE
        f = 1.0_wp - (a / r1_mag) * (1.0_wp - cos(DE))
        g = tof - sqrt(a ** 3 / mu_canonical) * (DE - sin(DE))
        gdot = 1.0_wp - (a / r2_mag) * (1.0_wp - cos(DE))
    else
        ! Parabolic case (not handled)
        print *, 'Parabolic orbit encountered in solve_battin.'
        soln%converged = .false.
        return
    endif

    ! Compute velocities
    soln%v1 = (r2 - f * r1) / g
    soln%v2 = (gdot * r2 - r1) / g
    soln%a = a
else
    print *, 'solve_battin did not converge within tolerance.'
endif
end subroutine solve_battin

function see(v) result(see_val)
implicit none
real(wp), intent(in) :: v
real(wp) :: see_val
! Local variables
real(wp) :: c(0:20)
real(wp) :: term, termold, del, delold, sum1, eta, sqrt_opv
integer :: i

! Coefficients
c(0) = 0.2_wp
c(1) = 9.0_wp / 35.0_wp
c(2) = 16.0_wp / 63.0_wp
c(3) = 25.0_wp / 99.0_wp
c(4) = 36.0_wp / 143.0_wp
c(5) = 49.0_wp / 195.0_wp
c(6) = 64.0_wp / 255.0_wp
c(7) = 81.0_wp / 323.0_wp
c(8) = 100.0_wp / 399.0_wp
c(9) = 121.0_wp / 483.0_wp
c(10) = 144.0_wp / 575.0_wp
c(11) = 169.0_wp / 675.0_wp
c(12) = 196.0_wp / 783.0_wp
c(13) = 225.0_wp / 899.0_wp
c(14) = 256.0_wp / 1023.0_wp
c(15) = 289.0_wp / 1155.0_wp
c(16) = 324.0_wp / 1295.0_wp
c(17) = 361.0_wp / 1443.0_wp
c(18) = 400.0_wp / 1599.0_wp
c(19) = 441.0_wp / 1763.0_wp
c(20) = 484.0_wp / 1935.0_wp

sqrt_opv = sqrt(1.0_wp + v)
eta = v / ((1.0_wp + sqrt_opv) ** 2)

! Initialize recursion
delold = 1.0_wp
termold = c(0)
sum1 = termold
i = 1

do while ((i <= 20) .and. (abs(termold) > 1.0e-12_wp))
    del = 1.0_wp / (1.0_wp + c(i) * eta * delold)
    term = termold * (del - 1.0_wp)
    sum1 = sum1 + term
    i = i + 1
    delold = del
    termold = term
end do

see_val = (1.0_wp / (8.0_wp * (1.0_wp + sqrt_opv))) * (3.0_wp + sum1 / (1.0_wp + eta * sum1))
end function see

function k_func(v) result(k_value)
implicit none
real(wp), intent(in) :: v
real(wp) :: k_value
! Local variables
real(wp) :: d(0:20)
real(wp) :: del, delold, term, termold, sum1
integer :: i

! Coefficients
d(0) = 1.0_wp / 3.0_wp
d(1) = 4.0_wp / 27.0_wp
d(2) = 8.0_wp / 27.0_wp
d(3) = 2.0_wp / 9.0_wp
d(4) = 22.0_wp / 81.0_wp
d(5) = 208.0_wp / 891.0_wp
d(6) = 340.0_wp / 1287.0_wp
d(7) = 418.0_wp / 1755.0_wp
d(8) = 598.0_wp / 2295.0_wp
d(9) = 700.0_wp / 2907.0_wp
d(10) = 928.0_wp / 3591.0_wp
d(11) = 1054.0_wp / 4347.0_wp
d(12) = 1330.0_wp / 5175.0_wp
d(13) = 1480.0_wp / 6075.0_wp
d(14) = 1804.0_wp / 7047.0_wp
d(15) = 1978.0_wp / 8091.0_wp
d(16) = 2350.0_wp / 9207.0_wp
d(17) = 2548.0_wp / 10395.0_wp
d(18) = 2968.0_wp / 11655.0_wp
d(19) = 3190.0_wp / 12987.0_wp
d(20) = 3658.0_wp / 14391.0_wp

! Initialize recursion
sum1 = d(0)
delold = 1.0_wp
termold = d(0)
i = 1

do while ((i <= 20) .and. (abs(termold) > 1.0e-12_wp))
    del = 1.0_wp / (1.0_wp + d(i) * v * delold)
    term = termold * (del - 1.0_wp)
    sum1 = sum1 + term
    i = i + 1
    delold = del
    termold = term
end do

k_value = sum1
end function k_func

! Calculate tau parameter (Battin's formulation)
tau = sqrt(r1_mag * r2_mag * (1.0_wp + cos(theta))) / (r1_mag + r2_mag)