58 lines
2.0 KiB
Julia
58 lines
2.0 KiB
Julia
function laguerre_conway(state::Vector{T}, μ::Float64, time::Float64) where T <: Real
|
||
|
||
n = 5 # Choose LaGuerre-Conway "n"
|
||
i = 0
|
||
|
||
r0 = state[1:3] # Are we in elliptical or hyperbolic orbit?
|
||
r0_mag = norm(r0)
|
||
v0 = state[4:6]
|
||
v0_mag = norm(v0)
|
||
σ0 = (r0 ⋅ v0)/√(μ)
|
||
a = 1 / ( 2/r0_mag - v0_mag^2/μ )
|
||
coeff = 1 - r0_mag/a
|
||
|
||
if a > 0 # Elliptical
|
||
ΔM = ΔE_new = √(μ) / sqrt(a^3) * time
|
||
ΔE = 1000
|
||
while abs(ΔE - ΔE_new) > 1e-10
|
||
ΔE = ΔE_new
|
||
F = ΔE - ΔM + σ0 / √(a) * (1-cos(ΔE)) - coeff * sin(ΔE)
|
||
dF = 1 + σ0 / √(a) * sin(ΔE) - coeff * cos(ΔE)
|
||
d2F = σ0 / √(a) * cos(ΔE) + coeff * sin(ΔE)
|
||
sign = dF >= 0 ? 1 : -1
|
||
ΔE_new = ΔE - n*F / ( dF + sign * √(abs((n-1)^2*dF^2 - n*(n-1)*F*d2F )))
|
||
i += 1
|
||
end
|
||
F = 1 - a/r0_mag * (1-cos(ΔE))
|
||
G = a * σ0/ √(μ) * (1-cos(ΔE)) + r0_mag * √(a) / √(μ) * sin(ΔE)
|
||
r = a + (r0_mag - a) * cos(ΔE) + σ0 * √(a) * sin(ΔE)
|
||
Ft = -√(a)*√(μ) / (r*r0_mag) * sin(ΔE)
|
||
Gt = 1 - a/r * (1-cos(ΔE))
|
||
else # Hyperbolic or Parabolic
|
||
ΔN = √(μ) / sqrt(-a^3) * time
|
||
ΔH = 0
|
||
ΔH_new = time < 0 ? -1 : 1
|
||
while abs(ΔH - ΔH_new) > 1e-10
|
||
ΔH = ΔH_new
|
||
F = -ΔN - ΔH + σ0 / √(-a) * (cos(ΔH)-1) + coeff * sin(ΔH)
|
||
dF = -1 + σ0 / √(-a) * sin(ΔH) + coeff * cos(ΔH)
|
||
d2F = σ0 / √(-a) * cos(ΔH) + coeff * sin(ΔH)
|
||
sign = dF >= 0 ? 1 : -1
|
||
ΔH_new = ΔH - n*F / ( dF + sign * √(abs((n-1)^2*dF^2 - n*(n-1)*F*d2F )))
|
||
i += 1
|
||
end
|
||
F = 1 - a/r0_mag * (1-cos(ΔH))
|
||
G = a * σ0/ √(μ) * (1-cos(ΔH)) + r0_mag * √(-a) / √(μ) * sin(ΔH)
|
||
r = a + (r0_mag - a) * cos(ΔH) + σ0 * √(-a) * sin(ΔH)
|
||
Ft = -√(-a)*√(μ) / (r*r0_mag) * sin(ΔH)
|
||
Gt = 1 - a/r * (1-cos(ΔH))
|
||
end
|
||
|
||
if i > 100
|
||
throw(ErrorException("LaGuerre-Conway did not converge!"))
|
||
end
|
||
|
||
return [ F*state[1:3] + G*state[4:6]; Ft*state[1:3] + Gt*state[4:6]]
|
||
|
||
end
|