pro traj

; Paul Withers, 2001.09.15
; LPL, Arizona

; To run code, enter the following at your IDL prompt
; .run traj
; traj

; Simplified version of trajectory integration code
; Uses entry state, constant scale height atmosphere, drag-only aerodynamics,
; constant drag coefficient, spherically symmetric planet to obtain trajectory

; Best documentation is
; http://www.lpl.arizona.edu/~withers/pppp/pdf/oureport.pdf

; Spacecraft are Mars Pathfinder (mpf), Galileo (gal), 
; Pioneer Venus large/sounder probe (pv), Beagle 2 (b2),
; and Huygens (huy)

forward_function get_planet, get_sct, get_entrystate

planet=''
while planet ne 'venus' and planet ne 'earth' and planet ne 'mars' $
 and planet ne 'jupiter' and planet ne 'titan' and planet ne 'user' do $
 read, planet, prompt='What planet? [venus,earth,mars,jupiter,titan,user] '

junk = get_planet(planet)
omega = junk(0)
rho0 = junk(1)
dsh = junk(2)
gm = junk(3)
rplanet = junk(4)
mean_molecular_mass = junk(5)

sct=''
while sct ne 'mpf' and sct ne 'gal' and sct ne 'b2' and sct ne 'huy' and $
 sct ne 'pv' and sct ne 'user' do $
 read, sct, prompt='What spacecraft? [mpf, gal, pv, b2, huy, user] '

junk = get_sct(sct)
mass = junk(0)
area = junk(1)

entry=''
while entry ne 'mpf' and entry ne 'gal' and entry ne 'b2' and entry ne 'huy' $
and entry ne 'pv' and entry ne 'user' do $
 read, entry, prompt='What entry state? [mpf, gal, pv, b2, huy, user] '

junk = get_entrystate(entry)
t0 = junk(0)
r0 = junk(1)+rplanet
lat0 = junk(2)
lon0 = junk(3)
speed0 = junk(4)
gamma0 = junk(5)
psi0 = junk(6)

n=long(0.)
read, n, prompt='How many timesteps? [Try 1000] '

dt=double(0.)
read, dt, prompt='Length of timesteps in seconds? [Try 1] '

cd=double(0.)
read, cd, prompt='Dimensionless drag coefficient? [2 is best] '

; Record whether we've hit planet or not
hitit = 0

; Setup many arrays used during integration
x_array = dblarr(n+1)
y_array = x_array
z_array = x_array
; position in inertial cartesian frame
vx_array = x_array
vy_array = x_array
vz_array = x_array
; velocity in inertial cartesian frame
height_array = x_array
lat_array = x_array
lon_array = x_array
; position in momentary spherical frame
vxrel_array = x_array
vyrel_array = x_array
vzrel_array = x_array
; velocity relative to atmosphere in inertial cartesian frame
aeroacc_array = x_array
; magnitude of aerodynamic acceleration
vrel_array = x_array
; magnitude of velocity relative to atmosphere
aeroaccinex_array = x_array
aeroacciney_array = x_array
aeroaccinez_array = x_array
; aerodynamic accelerations transformed into inertial cartesian frame
rho_array = x_array
; atmospheric density

vwind_array = dblarr(3,n+1)
; wind velocity (due to planetary rotation) at spacecraft in inertial cartesian frame
g_array = dblarr(3,n+1)
; gravitational acceleration at spacecraft in inertial cartesian frame

t_array = dindgen(n+1)*dt + t0
; time steps to be used in integration of trajectory
; Set t0 consistently with any other timed data that you have
; eg accelerometer data might be given with some other start time
; If you have no other timed data, this is irrelevant




colat0 = (double(90.)-lat0)*double(!pi/180.)
lon0   = lon0  *double(!pi/180.)
gamma0 = gamma0*double(!pi/180.)
psi0   = psi0  *double(!pi/180.)
; convert angles from degrees to radians

v_r = double(-1.) * speed0*sin(gamma0)
v_colat = double(-1.) * speed0*cos(gamma0)*cos(psi0)
v_lon = speed0*cos(gamma0)*sin(psi0)
; Transform velocity into momentary spherical frame

v_intx = v_r*sin(colat0)*cos(lon0) + v_colat*cos(colat0)*cos(lon0) - v_lon*sin(lon0)
v_inty = v_r*sin(colat0)*sin(lon0) + v_colat*cos(colat0)*sin(lon0) + v_lon*cos(lon0)
v_intz = v_r*cos(colat0)           - v_colat*sin(colat0)
; this transformation from Bradbury, Theoretical Mechanics, p64
; it's a standard one
; Momentary spherical frame to momentary cartesian frame

; then convert to frame1
vx1 = v_intx * cos(omega*t_array(0)) - v_inty * sin(omega*t_array(0))
vy1 = v_intx * sin(omega*t_array(0)) + v_inty * cos(omega*t_array(0))
vz1 = v_intz
; Transform from momentary cartesian frame to inertian cartesian frame

lon0 = lon0 + omega*t_array(0) ; going into frame 1
; Transfrom longitude from
; momentary spherical frame to inertial spherical frame

z1 = r0*cos(colat0)
y1 = r0*sin(colat0)*sin(lon0)
x1 = r0*sin(colat0)*cos(lon0)
; Transform position from inertial spherical frame into inertial cartesian frame

; Initialise some arrays
x_array(0) = x1
y_array(0) = y1
z_array(0) = z1
vx_array(0) = vx1
vy_array(0) = vy1
vz_array(0) = vz1

new_coord = cv_coord(from_rect=[x1,y1,z1], /to_sphere)
height_array(0) = new_coord(2) - rplanet
lat_array(0) = new_coord(1) * double(180. /!pi)
junk = (new_coord(0) - omega*t_array(0)) * double(180. /!pi)
while junk lt 0 do junk = junk + double(360.)
junk = double(junk - fix(junk)/360 * 360.)
lon_array(0) = junk



; Begin trajectory integration
i = long(1)
while i lt n+1 do begin


vwind = crossp([0.,0.,1.]*omega, [x_array(i-1),y_array(i-1),z_array(i-1)])
; calculate wind velocity at current position
vxrel = vx_array(i-1) - vwind(0)
vyrel = vy_array(i-1) - vwind(1)
vzrel = vz_array(i-1) - vwind(2)
; calculate spacecraft velocity relative to atmosphere
vrel = sqrt( vxrel^2 + vyrel^2 + vzrel^2 )
; calculate magnitudes of aerodynamic acceleration and relative velocity

aeroacc = cd * rho0*exp(double(-1.)*height_array(i-1)/dsh) * 0.5 * (area/mass) * vrel^2

aeroaccinex = -1. * aeroacc * vxrel / vrel
aeroacciney = -1. * aeroacc * vyrel / vrel
aeroaccinez = -1. * aeroacc * vzrel / vrel

; The above equations define my aerodynamic assumptions
; The only aerodynamic force is drag which is directed opposite to 
; velocity relative to atmosphere

g_r = double(-1.)*gm/((height_array(i-1)+rplanet)^2)
; Acceleration due to gravity in momentary spherical frame

g_intx=g_r*sin((double(90.)-lat_array(i-1))*double(!pi/180.))*cos(lon_array(i-1)*double(!pi/180.))
g_inty=g_r*sin((double(90.)-lat_array(i-1))*double(!pi/180.))*sin(lon_array(i-1)*double(!pi/180.))
g_intz=g_r*cos((double(90.)-lat_array(i-1))*double(!pi/180.))
; this transformation from Bradbury, Theoretical Mechanics, p64
; it's a standard one
; Transform from momentary spherical frame to momentary cartesian frame

gx1 = g_intx * cos(omega*t_array(i-1)) - g_inty * sin(omega*t_array(i-1))
gy1 = g_intx * sin(omega*t_array(i-1)) + g_inty * cos(omega*t_array(i-1))
gz1 = g_intz
; Transform from momentary cartesian frame to inertial cartesian frame

; update arrays
vwind_array(*,i-1) = [vwind(0), vwind(1), vwind(2)]
; store wind velocity in inertial cartesian frame at current position
vxrel_array(i-1) = vxrel
vyrel_array(i-1) = vyrel
vzrel_array(i-1) = vzrel
; store spacecraft velocity relative to atmosphere in inertial cartesian frame
aeroacc_array(i-1) = aeroacc
vrel_array(i-1) = vrel
; store magnitudes of aerodynamic acceleration and relative velocity
aeroaccinex_array(i-1) = aeroaccinex
aeroacciney_array(i-1) = aeroacciney
aeroaccinez_array(i-1) = aeroaccinez
; store xyz components of aerodynamic acceleration in inertial cartesian frame
g_array(*,i-1) = [gx1,gy1,gz1]
; store acceleration due to gravity in inertial cartesian frame at spacecraft position

x_array(i) = x_array(i-1) + vx_array(i-1)*dt
y_array(i) = y_array(i-1) + vy_array(i-1)*dt
z_array(i) = z_array(i-1) + vz_array(i-1)*dt

vx_array(i) = vx_array(i-1) + (aeroaccinex+gx1)*dt
vy_array(i) = vy_array(i-1) + (aeroacciney+gy1)*dt
vz_array(i) = vz_array(i-1) + (aeroaccinez+gz1)*dt
; simple first order integration technique
; update arrays

new_coord = cv_coord(from_rect=[x_array(i),y_array(i),z_array(i)], /to_sphere)
height_array(i) = new_coord(2) - rplanet
lat_array(i) = new_coord(1) * double(180. /!pi)
junk = (new_coord(0) - omega*t_array(i)) * double(180. /!pi)
while junk lt 0 do junk = junk + double(360.)
junk = double(junk - fix(junk)/360 * 360.)
lon_array(i) = junk

; update arrays

i = i+1
;print, i

; If trajectory hits planet, stop
if height_array(i-1) lt 0. then begin

print, 'Spacecraft hit the surface!'
print, 'There are ', i, ' useful elements at the start of the arrays'
hitit = 1
aa=findgen(i)
i=n+2
endif

endwhile

rho_array = rho0*exp(double(-1.)*height_array/dsh)

; If loop finishes without hitting planet, give warning
if hitit eq 0 then print, 'Spacecraft did not reach surface - increase n!'

if hitit eq 1 then plot, t_array(aa), height_array(aa), $
title='Entry Trajectory', xtitle='Time from t0 (sec)', ytitle='Altitude (m)', $
color=0, background=!d.n_colors-1, charsize=2, charthick=1.5, $
position=[0.25,0.15,0.9,0.9]

print, 'Useful arrays are:'
print, 'height_array 	- Altitude above surface (m)'
print, 'lat_array 	- Planetocentric north latitude (deg)'
print, 'lon_array 	- Planetocentric east longitude (deg)'
print, 't_array 	- Time since t0 (sec)'
print, 'vrel_array 	- Relative speed between spacecraft and atmosphere (m s-1)'
print, 'rho_array 	- Atmospheric density (kg m-3)'
print, 'Type "stop" as penultimate line of traj.pro to be able to acess these'

end