odevordinary.jl

using DifferentialEquations, BenchmarkTools
import ODE

function vop_prop( x, dt::Float64 )
    reltol = 1e-5
    abstol = 1e-13
    maxstep = 300.0 #sec

    p = ODEProblem( vopeom,
                    x,
                    (0.0, dt) )

            sol = solve(p,DP5(),
                        reltol = reltol,
                        abstol = abstol,
                        dtmax = maxstep,
                        dense = false,
                        save_timeseries=false,
                        )
    return sol[end]
end

function newton_raphson( x0::Float64, f::Function, df::Function, tol = 1e-10, maxIter = 25, SOR = 0.9)
    dx = 1.0
    counter = 1
    while abs(dx)>tol
        dx = SOR*f(x0)/df(x0)
        x0 -= dx
        counter += 1
        if counter > maxIter
            warn("Newton Raphson did not converge - try changing iteration limit")
            break
        end
    end
    return x0::Float64
end

function eq2rv_withaxes( x_eq::Vector{Float64} )
    n = x_eq[1]

    chi = x_eq[4]
    psi = x_eq[5]
    af = x_eq[2]
    ag = x_eq[3]
    meanlon = x_eq[6]

    NRf(x) = x - meanlon + ag*cos(x) - af*sin(x)
    NRdf(x) = 1.0 - ag*sin(x) - af*cos(x)
    F = newton_raphson( meanlon, NRf, NRdf ) # true longitude

    a = (398600/n^2)^(1/3)  # semimajor axis
    b = 1.0/(1+sqrt(1-ag^2-af^2))
    cF = cos(F)
    sF = sin(F)
    rmag = a * (1.0 - af*cF - ag*sF)

    X = a * ((1 - ag^2 * b)*cF + ag*af*b*sF - af)
    Y = a * ((1 - af^2 * b)*sF + ag*af*b*cF - ag)
    Xdot = n*a^2/rmag * (ag*af*b*cF - (1 - ag^2*b)*sF)
    Ydot = n*a^2/rmag * ((1 - af^2*b)*cF - ag*af*b*sF)

    # equinoctial reference axes (f,g,w)
    f = 1/(1+chi^2+psi^2).*[(1-chi^2+psi^2); 2*chi*psi; -2*chi]
    g = 1/(1+chi^2+psi^2).*[2*chi*psi; (1+chi^2-psi^2); 2*psi]

    r = X*f+Y*g
    v = Xdot*f+Ydot*g

    return ([r;v]::Vector{Float64},
            [X;Y;Xdot;Ydot]::Vector{Float64})
end

function mm2sma( mm::Float64 )
   return (398600.0/mm^2.)^(1/3.)
end

function get_equinoctial_frame( x::Vector{Float64} )
    c = 1./(1. + x[4]^2 + x[5]^2)
    fhat = c.*[1. - x[4]^2 + x[5]^2,
            2.*x[4]*x[5],
            -2.*x[4]]
    ghat = c.*[2.*x[4]*x[5],
            1. + x[4]^2 - x[5]^2,
            2.*x[5]]
    what = c.*[2.*x[4],
            -2.*x[5],
            1. - x[4]^2 - x[5]^2]
    return hcat(fhat,ghat,what)
end


function vopeom( t::Float64, x::Vector{Float64}, xdot::Vector{Float64})

    acc = zeros(3) #disturbing acceleration vector in inertial Cartesian coords

    (xECI, xCartesianEquinoctial) = eq2rv_withaxes(x[1:6])

    xdot[1:6] = gauss_vop( x[1:6], xECI, xCartesianEquinoctial, acc )
end

function gauss_vop( x::Vector{Float64}, xECI::Vector{Float64}, xCartEq::Vector{Float64}, Pbar::Vector{Float64} )

    dEQdxt = zeros(6,3)

    fgw = get_equinoctial_frame( x )
    fhat = fgw[:,1]
    ghat = fgw[:,2]
    what = fgw[:,3]
    a = mm2sma(x[1])
    beta = 1.0/(1 + sqrt( 1.0 - x[3]^2 - x[2]^2) )
    G = x[1] * a^2 * sqrt( 1.0 - x[3]^2 - x[2]^2 )

    # mean motion; see notes in astrodynamics.org
    dEQdxt[1,:] = -3/(x[1]*a^2).*xECI[4:6]
    #a_f
    dEQdxt[2,:] = -1.0/398600.0 * (xCartEq[2] * xCartEq[4] .* fhat - (2*xCartEq[1] * xCartEq[4] - xCartEq[3] * xCartEq[2]) .* ghat) - x[3]/G * (x[5] * xCartEq[2] - x[4] * xCartEq[1]) .* what
    #a_g
    dEQdxt[3,:] =  1.0/398600.0 * (( 2.*xCartEq[3] * xCartEq[2] - xCartEq[1] * xCartEq[4] ) .* fhat - xCartEq[1] * xCartEq[3] .* ghat ) + x[2]/G * (x[5] * xCartEq[2] - x[4] * xCartEq[1]) .* what
    #chi
    dEQdxt[4,:] = ( 1.0 + x[4]^2 + x[5]^2 )/(2. * G) * xCartEq[2] .* what
    #psi
    dEQdxt[5,:] = ( 1.0 + x[4]^2 + x[5]^2 )/(2. * G) * xCartEq[1] .* what
    #lambda
    dEQdxt[6,:] =  -2.0/(x[1] * a^2).*xECI[1:3] + beta .* (x[2].*vec(dEQdxt[3,:]) - x[3].*vec(dEQdxt[2,:])) + 1.0/(x[1]*a^2)*(x[5] * xCartEq[2] - x[4] * xCartEq[1]) .* what

    dEQdt = dEQdxt * Pbar

    dEQdt[6] = dEQdt[6] + x[1] # mean motion - two body dynamics

    return dEQdt
end


## ODE.jl

function vop_prop_old( x, dt::Float64 )
    reltol = 1e-5
    abstol = 1e-13
    maxstep = 300.0 #sec

    fcn(t,y) = vopeom_old(t,y)
            (tout,xout) = ODE.ode45( fcn,
                                x,
                                [0.0, dt],
                                reltol=reltol,
                                abstol=abstol,
                                maxstep=maxstep,
                                points = :specified )
            returnstate = xout[end]

    return returnstate
end

function vopeom_old( t::Float64, x::Vector{Float64})

    acc = zeros(3) #disturbing acceleration vector in inertial Cartesian coords

    (xECI, xCartesianEquinoctial) = eq2rv_withaxes(x[1:6])

    xdot = gauss_vop( x[1:6], xECI, xCartesianEquinoctial, acc )

    return xdot
end

x0 = [0.00113137
  0.0
  0.0
  0.898857
 -0.688568
  2.22543]

t = 1000.0

vop_prop(x0,t)
vop_prop_old(x0,t)

@benchmark vop_prop(x0,t)
@benchmark vop_prop_old(x0,t)

# t = 1000.0

# DP5

BenchmarkTools.Trial:
  memory estimate:  370.94 kb
  allocs estimate:  7156
  --------------
  minimum time:     610.443 μs (0.00% GC)
  median time:      746.170 μs (0.00% GC)
  mean time:        782.396 μs (5.26% GC)
  maximum time:     3.486 ms (77.28% GC)
  --------------
  samples:          6384
  evals/sample:     1
  time tolerance:   5.00%
  memory tolerance: 1.00%

  #ode45 direct

  BenchmarkTools.Trial:
  memory estimate:  258.03 kb
  allocs estimate:  5066
  --------------
  minimum time:     430.385 μs (0.00% GC)
  median time:      530.119 μs (0.00% GC)
  mean time:        555.616 μs (5.07% GC)
  maximum time:     6.499 ms (90.85% GC)
  --------------
  samples:          8987
  evals/sample:     1
  time tolerance:   5.00%
  memory tolerance: 1.00%


  #Tsit5

  BenchmarkTools.Trial:
  memory estimate:  376.10 kb
  allocs estimate:  7237
  --------------
  minimum time:     644.159 μs (0.00% GC)
  median time:      783.629 μs (0.00% GC)
  mean time:        823.532 μs (5.29% GC)
  maximum time:     3.671 ms (74.90% GC)
  --------------
  samples:          6065
  evals/sample:     1
  time tolerance:   5.00%
  memory tolerance: 1.00%

  ## ode45 common

  BenchmarkTools.Trial:
  memory estimate:  269.19 kb
  allocs estimate:  5289
  --------------
  minimum time:     515.459 μs (0.00% GC)
  median time:      633.040 μs (0.00% GC)
  mean time:        656.449 μs (4.29% GC)
  maximum time:     3.183 ms (76.96% GC)
  --------------
  samples:          7607
  evals/sample:     1
  time tolerance:   5.00%
  memory tolerance: 1.00%

# t = 20000.0

# DP5

BenchmarkTools.Trial:
  memory estimate:  3.21 mb
  allocs estimate:  63478
  --------------
  minimum time:     5.379 ms (0.00% GC)
  median time:      6.281 ms (0.00% GC)
  mean time:        6.475 ms (5.27% GC)
  maximum time:     13.131 ms (44.62% GC)
  --------------
  samples:          773
  evals/sample:     1
  time tolerance:   5.00%
  memory tolerance: 1.00%

# ode45 direct
BenchmarkTools.Trial:
  memory estimate:  3.21 mb
  allocs estimate:  64466
  --------------
  minimum time:     5.511 ms (0.00% GC)
  median time:      6.433 ms (0.00% GC)
  mean time:        6.606 ms (5.14% GC)
  maximum time:     9.271 ms (25.87% GC)
  --------------
  samples:          757
  evals/sample:     1
  time tolerance:   5.00%
  memory tolerance: 1.00%