kepler_classical Subroutine

    subroutine kepler_classical(x0, dt, mu, xf)

    use newton_module, only: newton

    implicit none

    real(wp),dimension(6),intent(in)  :: x0  !! initial position,velocity vector
    real(wp),intent(in)               :: dt  !! propagation time
    real(wp),intent(in)               :: mu  !! central body gravitational parameter
    real(wp),dimension(6),intent(out) :: xf  !! final position,velocity vector

    real(wp),dimension(3) :: r0  !! initial position vector
    real(wp),dimension(3) :: v0  !! initial velocity vector
    real(wp) :: de     !! eccentric anomaly difference
    real(wp) :: dh     !! hyperbolic anomaly difference
    integer  :: iflag  !! newton status flag
    real(wp) :: r,rmag,vmag,energy,a,sqrta,f,&
                g,ft,gt,sigma0,dm,dn,xs,fx,b,p,x,z,term

    real(wp),parameter :: parabolic_tol = 1.0e-12_wp !! zero tol for parabolic orbits (energy)
    real(wp),parameter :: ftol = 1.0e-12_wp          !! function tol for root finding
    real(wp),parameter :: xtol = 1.0e-12_wp          !! indep variable tol for root finding
    integer,parameter  :: max_iter = 1000            !! maximum number of iterations in newton

    ! check trivial case:
    if (dt==zero) then
        xf = x0
        return
    end if

    r0     = x0(1:3)
    v0     = x0(4:6)
    rmag   = norm2(r0)
    vmag   = norm2(v0)
    energy = (vmag * vmag / two - mu / rmag)
    sigma0 = dot_product(r0,v0) / sqrt(mu)

    ! if not parabolic, then compute semimajor axis
    if (abs(energy) > parabolic_tol) then
        a = -mu / two / energy
    end if

    if (energy < -parabolic_tol) then ! elliptical case

        sqrta = sqrt(a)
        dm = sqrt(mu / a**3) * dt
        de = dm

        call newton(de,kepde_,d_kepde_,ftol,xtol,max_iter,xs,fx,iflag)
        if (iflag<0) then
            write(error_unit,'(A)') 'Error in kepler_classical [elliptical]: newton did not converge'
            write(*,*) xs,fx,iflag
        end if

        de = xs
        r = a + (rmag - a) * cos(de) + sigma0 * sqrta * sin(de) ! eqn 4.42

        ! lagrange coefficients (Battin eqn 4.41)
        f  = one - a / rmag * (one - cos(de))
        g  = a * sigma0 / sqrt(mu) * (one - cos(de)) + rmag * sqrt(a / mu) * sin(de)
        ft = -sqrt(mu * a) / (r * rmag) * sin(de)
        gt = one - a / r * (one - cos(de))

    else if (energy > parabolic_tol) then ! hyperbolic case

        sqrta = sqrt(-a)
        dn = sqrt(-mu / a**3) * dt
        dh = sign(one,dt)      ! todo: need a better initial guess

        call newton(dh,kepdh_,d_kepdh_,ftol,xtol,max_iter,xs,fx,iflag)
        if (iflag<0) then
            write(error_unit,'(A)') 'Error in kepler_classical [hyperbola]: newton did not converge'
            write(*,*) xs,fx,iflag
        end if

        dh = xs
        r = a + (rmag - a) * cosh(dh) + sigma0 * sqrta * sinh(dh)

        ! lagrange coefficients (Battin eqn 4.62)
        f  = one - a / rmag * (one - cosh(dh))
        g  = a * sigma0 / sqrt(mu) * (one - cosh(dh)) + rmag * sqrt(-a / mu) * sinh(dh)
        ft = -sqrt(-mu * a) / (r * rmag) * sinh(dh)
        gt = one - a / r * (one - cosh(dh))

    else  ! parbolic case

        ! See Battin Section 4.2

        p    = two * rmag - sigma0**2
        B    = one / p**1.5_wp * ( sigma0 * (rmag + p) + three * sqrt(mu) * dt )
        term = B + sqrt(one+B*B)
        z    = (term)**(one/three) - (term)**(-one/three) ! Battin eqn 4.12 where z = tan(f/2)
        x    = sqrt(p) * z - sigma0
        r    = rmag + sigma0 * x + one/two * x**2

        f  = one - x**2 / two / rmag
        g  = x / two / sqrt(mu) * ( two * rmag + sigma0 * x )
        ft = - sqrt(mu) * x / rmag / r
        gt = one - x**2 / two / r

    end if

    ! results:
    xf(1:3) = f  * r0 + g  * v0
    xf(4:6) = ft * r0 + gt * v0

    contains

        ! function wrappers for newtons method:

        subroutine kepde_(x,f)
        implicit none
        real(wp),intent(in)  :: x  !! de
        real(wp),intent(out) :: f
        f = kepde(x, dm, sigma0, sqrta, a, rmag)
        end subroutine kepde_

        subroutine d_kepde_(x,f)
        implicit none
        real(wp),intent(in)  :: x  !! de
        real(wp),intent(out) :: f
        f = d_kepde(x, sigma0, sqrta, a, rmag)
        end subroutine d_kepde_

        subroutine kepdh_(x,f)
        implicit none
        real(wp),intent(in)  :: x  !! dh
        real(wp),intent(out) :: f
        f = kepdh(x, dn, sigma0, sqrta, a, rmag)
        end subroutine kepdh_

        subroutine d_kepdh_(x,f)
        implicit none
        real(wp),intent(in)  :: x  !! dh
        real(wp),intent(out) :: f
        f = d_kepdh(x, sigma0, sqrta, a, rmag)
        end subroutine d_kepdh_

    end subroutine kepler_classical