' demo_rkf78.bas March 27, 2017

' this program demonstrates how to interact
' with the rkf78.bas subroutine which
' solves a system of first-order nonlinear
' differential equations

' the system of differential equations is defined
' in a subroutine with the following syntax

' sub diffeq(t, y(), ydot())

' where t is the current time, y() is the current
' integration vector and ydot() is the system of
' first order differential equations evaluated at
' t and y()

' this example solves the differential equations
' of "Keplerian" (unperturbed) orbital motion

' MicroMite eXtreme version

'''''''''''''''''''''''''''

option default float

option base 1

dim ch(13), alph(13), beta(13, 12)

dim ri(3), vi(3), rf(3), vf(3)

dim yold(6), ynew(6), oevi(6), oevf(6)

dim neq as integer, ti, tf, h, tetol

const dtr = pi / 180.0, rtd = 180.0 / pi, pi2 = 2.0 * pi, pidiv2 = 0.5 * pi

' gravitational constant (kilometer^3/second^2)

const mu = 398600.4415

' define rkf78 integration coefficients

ch(6) = 34 / 105
ch(7) = 9 / 35
ch(8) = ch(7)
ch(9) = 9 / 280
ch(10) = ch(9)
ch(12) = 41 / 840
ch(13) = ch(12)

alph(2) = 2 / 27
alph(3) = 1 / 9
alph(4) = 1 / 6
alph(5) = 5 / 12
alph(6) = 0.5
alph(7) = 5 / 6
alph(8) = 1 / 6
alph(9) = 2 / 3
alph(10) = 1 / 3
alph(11) = 1
alph(13) = 1

beta(2, 1) = 2 / 27
beta(3, 1) = 1 / 36
beta(4, 1) = 1 / 24
beta(5, 1) = 5 / 12
beta(6, 1) = 0.05
beta(7, 1) = -25 / 108
beta(8, 1) = 31 / 300
beta(9, 1) = 2
beta(10, 1) = -91 / 108
beta(11, 1) = 2383 / 4100
beta(12, 1) = 3 / 205
beta(13, 1) = -1777 / 4100
beta(3, 2) = 1 / 12
beta(4, 3) = 1 / 8
beta(5, 3) = -25 / 16
beta(5, 4) = -beta(5, 3)
beta(6, 4) = 0.25
beta(7, 4) = 125 / 108
beta(9, 4) = -53 / 6
beta(10, 4) = 23 / 108
beta(11, 4) = -341 / 164
beta(13, 4) = beta(11, 4)
beta(6, 5) = 0.2
beta(7, 5) = -65 / 27
beta(8, 5) = 61 / 225
beta(9, 5) = 704 / 45
beta(10, 5) = -976 / 135
beta(11, 5) = 4496 / 1025
beta(13, 5) = beta(11, 5)
beta(7, 6) = 125 / 54
beta(8, 6) = -2 / 9
beta(9, 6) = -107 / 9
beta(10, 6) = 311 / 54
beta(11, 6) = -301 / 82
beta(12, 6) = -6 / 41
beta(13, 6) = -289 / 82
beta(8, 7) = 13 / 900
beta(9, 7) = 67 / 90
beta(10, 7) = -19 / 60
beta(11, 7) = 2133 / 4100
beta(12, 7) = -3 / 205
beta(13, 7) = 2193 / 4100
beta(9, 8) = 3
beta(10, 8) = 17 / 6
beta(11, 8) = 45 / 82
beta(12, 8) = -3 / 41
beta(13, 8) = 51 / 82
beta(10, 9) = -1 / 12
beta(11, 9) = 45 / 164
beta(12, 9) = 3 / 41
beta(13, 9) = 33 / 164
beta(11, 10) = 18 / 41
beta(12, 10) = 6 / 41
beta(13, 10) = 12 / 41
beta(13, 12) = 1

' define initial classical orbital elements

oevi(1) = 26553.175464
oevi(2) = 0.735
oevi(3) = 63.435 * dtr
oevi(4) = 270.0 * dtr
oevi(5) = 100.0 * dtr
oevi(6) = 45.0 * dtr

' compute initial state vector

orb2eci(mu, oevi(), ri(), vi())

print " "

print "initial orbital elements and state vector"
print "-----------------------------------------"
print " "

oeprint(mu, oevi())

print " "

svprint(ri(), vi())

' define number of differential equations

neq = 6

' initial step size guess (seconds)

h = 1.0

' truncation error tolerance

tetol = 1.0e-10

' load initial integration vector

for i% = 1 to 3

yold(i%) = ri(i%)

yold(i% + 3) = vi(i%)

next i%

' initial time (seconds)

ti = 0.0

' final time = ten keplerian orbital periods (seconds)

tau = pi2 * sqr(oevi(1) * oevi(1) * oevi(1) / mu)

tf = 10.0 * tau

' integrate equations of motion

rkf78(neq, ti, tf, h, tetol, yold(), ynew())

' unload final state vector

for i% = 1 to 3

rf(i%) = ynew(i%)

vf(i%) = ynew(i% + 3)

next i%

' compute and print final orbital elements

eci2orb(mu, rf(), vf(), oevf())

print " "
print " "

print "final orbital elements and state vector"
print "---------------------------------------"
print " "

oeprint(mu, oevf())

print " "

svprint(rf(), vf())

print " "
print " "

print "state vector differences (final - initial)"
print "------------------------------------------"
print " "

print "position vector x-component ", str$(1000.0 * (rf(1) - ri(1)), 0, 6), " meters"
print "position vector y-component ", str$(1000.0 * (rf(2) - ri(2)), 0, 6), " meters"
print "position vector z-component ", str$(1000.0 * (rf(3) - ri(3)), 0, 6), " meters"

print " "

print "velocity vector x-component ", str$(1000.0 * (vf(1) - vi(1)), 0, 10), " meters/second"
print "velocity vector y-component ", str$(1000.0 * (vf(2) - vi(2)), 0, 10), " meters/second"
print "velocity vector z-component ", str$(1000.0 * (vf(3) - vi(3)), 0, 10), " meters/second"

print " "

end

'''''''''''''''''''''''''''''''''''''''''''
'''''''''''''''''''''''''''''''''''''''''''

sub rkf78 (neq, ti, tf, h, tetol, xi(), xf())

' solve first order system of differential equations

' Runge-Kutta-Fehlberg 7(8) method

' input

' neq = number of differential equations
' ti = initial simulation time (seconds)
' tf = final simulation time (seconds)
' h = initial guess for integration step size
' tetol = truncation error tolerance (non-dimensional)
' xi() = integration vector at time = ti

' output

' xf() = integration vector at time = tf

'''''''''''''''''''''''''''''''''''''''''

local xdot(neq), xwrk(neq), f(neq, 13)

local sdt, dt, temp, twrk

' compute integration "direction"

sdt = sgn(tf - ti)

dt = abs(h) * sdt

do

' load "working" time and integration vector

twrk = ti

for i% = 1 to neq

xwrk(i%) = xi(i%)

next i%

' check for last delta-t

if (abs(dt) > abs(tf - ti)) then dt = tf - ti

' check for end of integration period

if (abs(ti - tf) < 1.0e-12) then

for i% = 1 to neq

xf(i%) = xi(i%)

next i%

' finished - exit do loop

exit do

end if

' evaluate equations of motion

diffeq(ti, xi(), xdot())

for i% = 1 to neq

f(i%, 1) = xdot(i%)

next i%

' compute solution

for k% = 2 to 13

kk% = k% - 1

for i% = 1 to neq

temp = 0.0

for j% = 1 to kk%

temp = temp + beta(k%, j%) * f(i%, j%)

next j%

xi(i%) = xwrk(i%) + dt * temp

next i%

ti = twrk + alph(k%) * dt

diffeq(ti, xi(), xdot())

for j% = 1 to neq

f(j%, k%) = xdot(j%)

next j%

next k%

for i% = 1 to neq

temp = 0.0

for l% = 1 to 13

temp = temp + ch(l%) * f(i%, l%)

next l%

xi(i%) = xwrk(i%) + dt * temp

next i%

' truncation error calculations

xerr = tetol

for i% = 1 to neq

ter = abs((f(i%, 1) + f(i%, 11) - f(i%, 12) - f(i%, 13)) * ch(12) * dt)

tol = abs(xi(i%)) * tetol + tetol

tconst = ter / tol

if (tconst > xerr) then xerr = tconst

next i%

' compute new step size

dt = 0.8 * dt * (1.0 / xerr) ^ (1.0 / 8.0)

if (xerr > 1.0) then

' reject current step and try again

ti = twrk

for i% = 1 to neq

xi(i%) = xwrk(i%)

next i%

else

''''''''''''''''''''''''''''''''''''
' accept current step '
''''''''''''''''''''''''''''''''''''
' perform graphics, additional '
' calculations, etc. at this point '
''''''''''''''''''''''''''''''''''''

end if

loop

end sub

'''''''''''''''''''''''''
'''''''''''''''''''''''''

sub diffeq (t, y(), ydot())

' first order equations of orbital motion

' Keplerian motion - no perturbations

' input

' t = simulation time (seconds)
' y() = state vector

' output

' ydot() = integration vector

''''''''''''''''''''''''''''''

local rmag, rcubed

rmag = sqr(y(1) * y(1) + y(2) * y(2) + y(3) * y(3))

rcubed = rmag * rmag * rmag

' integration vector

ydot(1) = y(4)

ydot(2) = y(5)

ydot(3) = y(6)

ydot(4) = -mu * y(1) / rcubed

ydot(5) = -mu * y(2) / rcubed

ydot(6) = -mu * y(3) / rcubed

end sub

'''''''''''''''''''''''''''''''
'''''''''''''''''''''''''''''''

sub eci2orb (mu, r(), v(), oev())

' convert eci state vector to six classical orbital
' elements via equinoctial elements

' input

' mu = central body gravitational constant (km**3/sec**2)
' r() = eci position vector (kilometers)
' v() = eci velocity vector (kilometers/second)

' output

' oev(1) = semimajor axis (kilometers)
' oev(2) = orbital eccentricity (non-dimensional)
' (0 <= eccentricity < 1)
' oev(3) = orbital inclination (radians)
' (0 <= inclination <= pi)
' oev(4) = argument of perigee (radians)
' (0 <= argument of perigee <= 2 pi)
' oev(5) = right ascension of ascending node (radians)
' (0 <= raan <= 2 pi)
' oev(6) = true anomaly (radians)
' (0 <= true anomaly <= 2 pi)

'''''''''''''''''''''''''''''''''''''''

local rmag, vmag, sma, p, q, const1

local h, xk, x1, y1, eccm, inc, xlambdat

local raan, argper, tanom

local rhat(3), hhat(3), vtmp(3), ecc(3)

local hv(3), fhat(3), ghat(3)

' position and velocity magnitude

rmag = vecmag(r())

vmag = vecmag(v())

' position unit vector

uvector(r(), rhat())

' angular momentum vectors

vcross(r(), v(), hv())

uvector(hv(), hhat())

' eccentricity vector

for i% = 1 to 3

vtmp(i%) = v(i%) / mu

next i%

vcross(vtmp(), hv(), ecc())

for i% = 1 to 3

ecc(i%) = ecc(i%) - rhat(i%)

next i%

' semimajor axis

sma = 1.0 / (2.0 / rmag - vmag * vmag / mu)

p = hhat(1) / (1.0 + hhat(3))

q = -hhat(2) / (1.0 + hhat(3))

const1 = 1.0 / (1.0 + p * p + q * q)

fhat(1) = const1 * (1.0 - p * p + q * q)
fhat(2) = const1 * 2.0 * p * q
fhat(3) = -const1 * 2.0 * p

ghat(1) = const1 * 2.0 * p * q
ghat(2) = const1 * (1.0 + p * p - q * q)
ghat(3) = const1 * 2.0 * q

h = vdot(ecc(), ghat())

xk = vdot(ecc(), fhat())

x1 = vdot(r(), fhat())

y1 = vdot(r(), ghat())

' orbital eccentricity

eccm = sqr(h * h + xk * xk)

' orbital inclination

inc = 2.0 * atn(sqr(p * p + q * q))

' true longitude

xlambdat = atan3(y1, x1)

' check for equatorial orbit

if (inc > 1.0e-8) then

raan = atan3(p, q)

else

raan = 0.0

end if

' check for circular orbit

if (eccm > 1.0e-8) then

argper = modulo(atan3(h, xk) - raan)

else

argper = 0.0

end if

' true anomaly (radians)

tanom = modulo(xlambdat - raan - argper)

' load orbital element vector

oev(1) = sma
oev(2) = eccm
oev(3) = inc
oev(4) = argper
oev(5) = raan
oev(6) = tanom

end sub

''''''''''''''''''''''''''''''
''''''''''''''''''''''''''''''

sub orb2eci(mu, oev(), r(), v())

' convert classical orbital elements to state vector

' input

' mu = gravitational constant (km**3/sec**2)
' oev(1) = semimajor axis (kilometers)
' oev(2) = orbital eccentricity (non-dimensional)
' (0 <= eccentricity < 1)
' oev(3) = orbital inclination (radians)
' (0 <= inclination <= pi)
' oev(4) = argument of perigee (radians)
' (0 <= argument of perigee <= 2 pi)
' oev(5) = right ascension of ascending node (radians)
' (0 <= raan <= 2 pi)
' oev(6) = true anomaly (radians)
' (0 <= true anomaly <= 2 pi)

' output

' r() = position vector (kilometers)
' v() = velocity vector (kilometers/second)

''''''''''''''''''''''''''''''''''''''''''''

local sma, ecc, inc, argper, raan, tanom

local slr, rm, arglat, sarglat, carglat

local c4, c5, c6, sinc, cinc, sraan, craan

' unload orbital elements array

sma = oev(1)
ecc = oev(2)
inc = oev(3)
argper = oev(4)
raan = oev(5)
tanom = oev(6)

slr = sma * (1.0 - ecc * ecc)

rm = slr / (1.0 + ecc * cos(tanom))

arglat = argper + tanom

sarglat = sin(arglat)
carglat = cos(arglat)

c4 = sqr(mu / slr)
c5 = ecc * cos(argper) + carglat
c6 = ecc * sin(argper) + sarglat

sinc = sin(inc)
cinc = cos(inc)

sraan = sin(raan)
craan = cos(raan)

' position vector

r(1) = rm * (craan * carglat - sraan * cinc * sarglat)
r(2) = rm * (sraan * carglat + cinc * sarglat * craan)
r(3) = rm * sinc * sarglat

' velocity vector

v(1) = -c4 * (craan * c6 + sraan * cinc * c5)
v(2) = -c4 * (sraan * c6 - craan * cinc * c5)
v(3) = c4 * c5 * sinc

end sub

''''''''''''''''''''
''''''''''''''''''''

sub oeprint(mu, oev())

' print classical orbital elements subroutine

if (oev(1) > 0.0) then

tau = pi2 * sqr(oev(1) * oev(1) * oev(1) / mu)

else

tau = 0.0

end if

print "semimajor axis ", str$(oev(1), 0, 4), " kilometers"
print "eccentricity ", str$(oev(2), 0, 10)
print "orbital inclination ", str$(rtd * oev(3), 0, 4), " degrees"
print "argument of perigee ", str$(rtd * oev(4), 0, 4), " degrees"
print "RAAN ", str$(rtd * oev(5), 0, 4), " degrees"
print "true anomaly ", str$(rtd * oev(6), 0, 4), " degrees"
print " "
print "orbital period ", str$(tau / 60.0, 0, 4), " minutes"

end sub

'''''''''''''''''''
'''''''''''''''''''

sub svprint(r(), v())

' print position and velocity vectors subroutine

local rmag, vmag

print "position vector x-component ", str$(r(1), 0, 6), " kilometers"
print "position vector y-component ", str$(r(2), 0, 6), " kilometers"
print "position vector z-component ", str$(r(3), 0, 6), " kilometers"

rmag = vecmag(r())

print " "

print "position vector magnitude ", str$(rmag, 0, 6), " kilometers"

print " "

print "velocity vector x-component ", str$(v(1), 0, 10), " kilometers/second"
print "velocity vector y-component ", str$(v(2), 0, 10), " kilometers/second"
print "velocity vector z-component ", str$(v(3), 0, 10), " kilometers/second"

vmag = vecmag(v())

print " "

print "velocity vector magnitude ", str$(vmag, 0, 10), " kilometers/second"


end sub

'''''''''''''''''''''''''
'''''''''''''''''''''''''

function modulo(x) as float

' modulo 2 pi function

''''''''''''''''''''''

local a

a = x - pi2 * fix(x / pi2)

if (a < 0.0) then

a = a + pi2

end if

modulo = a

end function

'''''''''''''''''''''''''''
'''''''''''''''''''''''''''

function atan3(a, b) as float

' four quadrant inverse tangent function

' input

' a = sine of angle
' b = cosine of angle

' output

' atan3 = angle (0 =< atan3 <= 2 * pi; radians)

''''''''''''''''''''''''''''''''''''''''''''''''

local c

if (abs(a) < 1.0e-10) then

atan3 = (1.0 - sgn(b)) * pidiv2

exit function

else

c = (2.0 - sgn(a)) * pidiv2

endif

if (abs(b) < 1.0e-10) then

atan3 = c

exit function

else

atan3 = c + sgn(a) * sgn(b) * (abs(atn(a / b)) - pidiv2)

endif

end function

'''''''''''''''''''''''''''
'''''''''''''''''''''''''''

function vecmag(a()) as float

' vector magnitude function

' input

' { a } = column vector ( 3 rows by 1 column )

' output

' vecmag = scalar magnitude of vector { a }

vecmag = sqr(a(1) * a(1) + a(2) * a(2) + a(3) * a(3))

end function

''''''''''''''''''''
''''''''''''''''''''

sub uvector (a(), b())

' unit vector subroutine

' input

' a = column vector (3 rows by 1 column)

' output

' b = unit vector (3 rows by 1 column)

'''''''''''''''''''''''''''''''''''''''

local amag

amag = vecmag(a())

for i% = 1 to 3

if (amag <> 0.0) then

b(i%) = a(i%) / amag

else

b(i%) = 0.0

end if

next i%

end sub

''''''''''''''''''''''''''''''
''''''''''''''''''''''''''''''

function vdot(a(), b()) as float

' vector dot product function

' c = { a } dot { b }

' input

' n% = number of rows
' { a } = column vector with n rows
' { b } = column vector with n rows

' output

' vdot = dot product of { a } and { b }

''''''''''''''''''''''''''''''''''''''''

local c = 0.0

for i% = 1 to 3

c = c + a(i%) * b(i%)

next i%

vdot = c

end function

'''''''''''''''''''''''
'''''''''''''''''''''''

sub vcross(a(), b(), c())

' vector cross product subroutine

' { c } = { a } x { b }

' input

' { a } = vector a ( 3 rows by 1 column )
' { b } = vector b ( 3 rows by 1 column )

' output

' { c } = { a } x { b } ( 3 rows by 1 column )

c(1) = a(2) * b(3) - a(3) * b(2)
c(2) = a(3) * b(1) - a(1) * b(3)
c(3) = a(1) * b(2) - a(2) * b(1)

end sub