// program demo_earth2mars  HP Prime

// October 1, 2022

// earth-to-mars ballistic trajectory

// Orbital Mechanics with the HP Prime

//////////////////////////////////////

// gravitational constant of the sun (kilometers^3/seconds^2)

EXPORT mu := 132712441933;      

// astronomical unit (kilometers)

EXPORT aunit := 149597870.691;

EXPORT statev := makemat(0, 12, 11);

// support subroutines

lambert();

ziter();

vinitial();

planet();

kepler1();

twobody2();

orb2eci();

eci2orb1();

julian();

gdate();

jd2str();

rvprint();

svprint();

oeprint();

v3mag();

atan3();

// main program

EXPORT demo_earth2mars()

BEGIN

LOCAL sv_depart := [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

LOCAL sv_arrive := [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

LOCAL sv2body := [0, 0, 0, 0, 0, 0];

LOCAL oev_depart := [0, 0, 0, 0, 0, 0];

LOCAL oev_arrive := [0, 0, 0, 0, 0, 0];

LOCAL oevtoi := [0, 0, 0, 0, 0, 0];

LOCAL oevtof := [0, 0, 0, 0, 0, 0];

LOCAL ri := [0, 0, 0];

LOCAL vi := [0, 0, 0];

LOCAL rf := [0, 0, 0];

LOCAL vf := [0, 0, 0];

LOCAL rito := [0, 0, 0];

LOCAL vito := [0, 0, 0];

LOCAL rfto := [0, 0, 0];

LOCAL vfto := [0, 0, 0];

LOCAL deltav1 := [0, 0, 0];

LOCAL deltav2 := [0, 0, 0];

LOCAL deltavm1, deltavm2, revmax;

LOCAL i, j, nsol, tt_days, direct, tof;

LOCAL ip_depart, ip_arrive;

LOCAL month, day, year, jd_depart, jd_arrive;

LOCAL dtr := pi / 180.0;

// departure calendar date

month := 6;

day := 1;

year := 2003;

// departure utc julian day

jd_depart := julian(month, day, year);

// earth is the departure planet

ip_depart := 3;

sv_depart := planet(ip_depart, jd_depart);

for i from 1 to 6 do

   oev_depart(i) := sv_depart(i + 6);

end;

// arrival calendar date

month := 12;

day := 27;

year := 2003;

// arrival utc julian day

jd_arrive := julian(month, day, year);

// mars is the arrival planet

ip_arrive := 4;

sv_arrive := planet(ip_arrive, jd_arrive);

for i from 1 to 6 do

   oev_arrive(i) := sv_arrive(i + 6);

end;

// transfer time in days

tt_days := jd_arrive - jd_depart;

// orbital direction (1 = posigrade, 2 = retrograde)

direct := 1;

// maximum number of transfer orbits around the sun

revmax := 0;

// time-of-flight in seconds

tof := 86400.0 * tt_days;

// solve earth-to-mars orbit transfer

for i from 1 to 3 do

    ri(i) := sv_depart(i);

    vi(i) := sv_depart(i + 3);
    
    rf(i) := sv_arrive(i);

    vf(i) := sv_arrive(i + 3);

end;

print();

print("demo_earth2mars");

print("\nEarth-to-Mars orbit transfer");

print("\ndeparture calendar date and time");

jd2str(jd_depart);

print("\narrival calendar date and time");

jd2str(jd_arrive);

print("\ntime-of-flight = " + tt_days + " days");

// compute lambert solution

nsol := lambert(ri, rf, tof, direct, revmax);

for i from 1 to 3 do

    rito(i) := ri(i);

    vito(i) := statev(i + 3, 1);

end;

for i from 1 to 6 do

    oevtoi(i) := statev(i + 6, 1);

end;

print("\norbital elements at departure");

oeprint(oev_depart);

// rvprint(ri, vi);

print("\norbital elements after departure deltav");

oeprint(oevtoi);

// rvprint(rito, vito);

// compute first deltav vector and magnitude (meters/second)
  
deltav1(1) := 1000.0 * (vito(1) - vi(1));
deltav1(2) := 1000.0 * (vito(2) - vi(2));
deltav1(3) := 1000.0 * (vito(3) - vi(3));
  
deltavm1 := v3mag(deltav1);

print("\neci components of departure deltav (meters/second)");

print("\ndeltav_x = " + deltav1(1));
print("deltav_y = " + deltav1(2));
print("deltav_z = " + deltav1(3));

print("\ndeltav magnitude = " + round(deltavm1, 4) + " meters/second");

// two-body propagation to second deltav

sv2body := twobody2(tof, rito, vito);

for i from 1 to 3 do

    rfto(i) := sv2body(i);

    vfto(i) := sv2body(i + 3);

end;

oevtof := eci2orb1(rfto, vfto);

print("\norbital elements prior to arrival deltav");

oeprint(oevtof);

// rvprint(rfto, vfto);

// compute second deltav vector and magnitude (meters/second)
  
deltav2(1) := 1000.0 * (vf(1) - vfto(1));
deltav2(2) := 1000.0 * (vf(2) - vfto(2));
deltav2(3) := 1000.0 * (vf(3) - vfto(3));
  
deltavm2 := v3mag(deltav2);

print("\neci components of arrival deltav (meters/second)");

print("\ndeltav_x = " + deltav2(1));
print("deltav_y = " + deltav2(2));
print("deltav_z = " + deltav2(3));

print("\ndeltav magnitude = " + round(deltavm2, 4) + " meters/second");

print("\ntotal deltav = " + round(deltavm1 + deltavm2, 4) + " meters/second");

print("\norbital elements after arrival deltav");

oeprint(oev_arrive);

// rvprint(rf, vf);

END;

////////////////////////////////////
////////////////////////////////////

lambert(ri, rf, tof, direct, revmax)

// solve Lambert's orbital two point boundary value problem

// input

//  ri     := initial ECI position vector (kilometers)
//  rf     := final ECI position vector (kilometers)
//  tof    := time of flight (seconds)
//  direct := transfer direction (1 := posigrade, -1 := retrograde)
//  revmax := maximum number of complete orbits

// output

//  nsol   := number of solutions
//  statev := matrix of solutions

//  statev(1, nsol) = position vector x component
//  statev(2, nsol) = position vector y component
//  statev(3, nsol) = position vector z component
//  statev(4, nsol) = velocity vector x component
//  statev(5, nsol) = velocity vector y component
//  statev(6, nsol) = velocity vector z component
//  statev(7, nsol) = semimajor axis
//  statev(8, nsol) = orbital eccentricity
//  statev(9, nsol) = orbital inclination
//  statev(10, nsol) = argument of perigee
//  statev(11, nsol) = right ascension of the ascending node
//  statev(12, nsol) = true anomaly
  
// Orbital Mechanics with the HP Prime

//////////////////////////////////////

BEGIN

LOCAL r1 := [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

LOCAL r2 := [0, 0, 0];

LOCAL r1wrk := [0, 0, 0];

LOCAL chord := [0, 0, 0];

LOCAL r1xr2 := [0, 0, 0];

LOCAL rr1 := [0, 0, 0];

LOCAL vr1 := [0, 0, 0];

LOCAL oev := [0, 0, 0, 0, 0, 0];

LOCAL vwrk := [0, 0, 0, 0];

LOCAL pi2 := 2.0 * pi;

LOCAL pidiv2 := 0.5 * pi;

LOCAL xktab := [1, -1];

LOCAL index, z, zl, zmax, zmin;

LOCAL maxlm, lamda, lambda, maxlam;

LOCAL lammx, lammax, ms, w, xk, dndz, nz;

LOCAL tl, dndz, dn2dz2, az, sqaz;

LOCAL f1, f2, t1, t2, t3;

LOCAL z0, z1, z2, z3, k, xk;

LOCAL niter, amin, tmin, tmin1;

LOCAL i, ii, j, verr, ms, w, nc, xp1;

LOCAL ns, mpft, w2, w3, c, s, del;

LOCAL csq, eps, r1dotr2, r1sq, r2sq;

LOCAL seps, ceps, r1m, r2m;

LOCAL sqrt2 := sqrt(2.0);

LOCAL nsol := 0;

LOCAL bkep1 := 0;

LOCAL zerr := 0;

LOCAL zsaved := 0;

LOCAL test := 1;

// load "working" vectors

for i from 1 to 3 do

    r1(i) := ri(i);
    
    r2(i) := rf(i);
    
    chord(i) := r1(i) - r2(i);

    r1wrk(i) := r1(i);
    
end;

r1sq := dot(r1wrk, r1wrk);
   
r1m := sqrt(r1sq);
     
r2sq := dot(r2, r2);
   
r2m := sqrt(r2sq);
     
r1dotr2 := dot(r1wrk, r2);
   
ceps := r1dotr2 / r1m / r2m;

seps := sqrt(1.0 - ceps ^ 2);

r1xr2 := cross(r1wrk, r2);
     
if ((r1xr2(3) * direct) < 0) then

   seps := -seps;
   
end;   
     
eps := atan3(seps, ceps);
     
csq := dot(chord, chord);
     
c := sqrt(csq);

s := 0.5 * (r1m + r2m + c);

w2 := (r1m + r2m - c) / (2.0 * s);

w := sign(pi - eps) * sqrt(w2);

w3 := w2 * w;

ns := sqrt(mu / s ^ 3);

ms := ns * tof;

mpft := sqrt2 / 3 * (1.0 - w3);

del := sqrt2 / 100 * (1.0 - w3 * w2);

xk := 1;

lamda := 0;

if ((ms - mpft) <= 0) then

   zmin := -1.0e20;
   
   zmax := 0;
   
   z0 := -1;
     
   if (abs(ms - mpft) < del) then
    
      z0 := -0.0001;
      
   end;
     
   nc := 2;
   
else

   zmin := 0.0;
   
   zmax := 1.0;
   
   z0 := 0.75;
   
   if (abs(ms - mpft) < del) then
   
      z0 := 0.0001;
      
   end;   
     
   xp1 := (pidiv2 - asin(w) + w * sqrt(1.0 - w2)) / sqrt2;
     
   if (ms > xp1) then
   
      xk := -1;
      
   end;   
     
   nc := 1;
   
end;

z := z0;

vwrk := ziter(z, zmax, zmin, lamda, ms, nc, w, xk);

dndz := vwrk(1);

nz := vwrk(2);

z := vwrk(3);

zerr := vwrk(4);

if (zerr = 0) then

   vwrk := vinitial(c, s, w, xk, z, r1, r2, seps, ceps, r1m, r2m);

   r1(4) := vwrk(1);
   r1(5) := vwrk(2);
   r1(6) := vwrk(3);
        
   verr := vwrk(4);
        
   if (verr = 1) then
   
      print("verr in lambert");

      return;
      
   end;

   for ii from 1 to 3 do
   
      rr1(ii) := r1(ii);
      
      vr1(ii) := r1(ii + 3);
      
   end;

   oev := eci2orb1(rr1, vr1);
     
   if (abs(oev(2) - 1.0) <= 0.000001) then
   
      oev(2) := 1.0;
      
      oev(1) := r1m * (1.0 + cos(oev(6)));
      
      nc := 3;
      
   end;
   
   for ii from 1 to 6 do
   
      r1(6 + ii) := oev(ii);
      
   end;

   for i from 1 to 12 do
   
       statev(i, 1) := r1(i);
       
   end;

   nsol := 1;
   
end;

if ((revmax = 0) or (nc <> 1)) then

   return nsol;
   
end;   

amin := s / 2;

pmin := pi2 * sqrt(amin ^ 3 / mu);

lammax := floor(tof / pmin);
     
if (lammax = 0) then

   return;
   
end;   

tmin := xp1 / ns;

tmin1 := lammax * pmin + tmin;
     
if (tof < tmin1) then

   niter := 1;
   
   z := 0.75;
   
   xk := 1;
   
   k := pi * (0.5 * (1 - xk) + lammax);

   test := 1;
   
   while (test = 1) do
   
      az := abs(z);
      
      sqaz := sqrt(az);
     
      z1 := sqrt(1 - z);
      
      z2 := sqrt(1 - w2 * z);
      
      z3 := w * sqrt(az);
     
      f1 := asin(sqaz);
      
      f2 := asin(z3);

      nz := (k + xk * (f1 - z1 * az) - f2 + z3 * z2) / sqrt2 / z / sqaz;

      dndz := (xk / z1 - w3 / z2 - 3 / sqrt2 * nz) / z * (1 / sqrt2);
     
      if ((abs(dndz) <= 0.000001) or (abs(zsaved - z) < 0.00000001)) then
      
         break;
         
      end;   
     
      if (niter >= 50) then

         print("niter > 50 in lambert");
      
         lammax := lammax - 1;
         
         break;
         
      end;

      dn2dz2 := (xk / z1 ^ 3 - w2 * w3 / z2 ^ 3 - 5 * sqrt2 * dndz) / 2 / sqrt2 / az;

      zsaved := z;

      z := z - dndz / dn2dz2;
     
      if (z > zmax) then
      
         z := 0.5 * (zmax + zsaved);
         
      end;
      
      if (z < zmin) then
      
         z := 0.5 * (zmin + zsaved);
         
      end;

      niter := niter + 1;

   end;

   if (niter < 50) then
   
      zl := z;
      
      tl := nz / ns;
      
      if ((tof - tl) <= 0) then
      
         lammax := lammax - 1;
         
      else
      
         bkep1 := 1;
         
      end;
      
   end;
   
end;

lammx := 5;
   
if (lammax < lammx) then

   lammx := lammax;
   
end;

if (revmax < lammx) then

   lammx := revmax;
   
end;

nsol := 2 * lammx + 1;
    
maxlam := lammx + 0.5;
   
if (maxlam = 0) then

   return;
   
end;   
   
maxlm := floor(lammax + 0.5);

for lambda from 1 to maxlam do

    lamda := lambda;
    
    for k from 1 to 2 do
    
        xk := xktab(k);
        
        if ((lambda <> maxlm) or (bkep1 <> 1)) then
        
           // null
           
        else
        
           xk := 1;
           
           if (k = 1) then
           
              zmax := zl;
              
              z0 := 0.75 * zl;
              
           else
           
              zmin := zl;
              
              zmax := 1;
              
              z0 := 0.5 * (zl + zmax);
              
           end;
           
        end;

        z := z0;
        
        vwrk := ziter(z, zmax, zmin, lamda, ms, nc, w, xk);
        
        dndz := vwrk(1);

        nz := vwrk(2);

        z := vwrk(3);

        zerr := vwrk(4);
                
        if (zerr = 1) then

           print("zerr in lambert");
        
           return;
           
        end;   

        vwrk := vinitial(c, s, w, xk, z, r1, r2, seps, ceps, r1m, r2m);
        
        r1(1) := vwrk(1);
        r1(2) := vwrk(2);
        r1(3) := vwrk(3);
        
        verr := vwrk(4);
        
        if (verr = 1) then
        
           print("verr in lambert");

           return;
           
        end;   

        for ii from 1 to 3 do
        
           rr1(ii) := r1(ii);
           
           vr1(ii) := r1(ii + 3);
           
        end;

        oev := eci2orb1(rr1, vr1);

        for ii from 1 to 6 do
        
            r1(6 + ii) := oev(ii);
            
        end;

        index := 2 * lambda + k - 1;
    
        for i from 1 to 12 do
        
            statev(i, index) := r1(i);
            
        end;
         
    end;
    
end;

end;

//////////////////////////////////////////////////////
//////////////////////////////////////////////////////

vinitial(c, s, w, xk, z, r1, r2, seps, ceps, r1m, r2m)

// lambert support subroutine

/////////////////////////////

BEGIN

LOCAL vwrk := [0, 0, 0, 0];

LOCAL i, verr, z1, z2, p, d, f, g;
                      
// initial velocity

verr := 0;

if (abs(seps) < 0.000001) then

   verr := 1;

   print("verr in vinitial");
   
   return;
   
end;

z1 := sqrt(1.0 - z);

z2 := sqrt(1.0 - z * w * w);

p := 2.0 / c * (s - r1m) * (s - r2m) * ((z2 + xk * w * z1) ^ 2) / c * s;

d := sqrt(mu) / sqrt(p) / seps;

f := d * (1 - ceps - p / r2m) / r1m;

g := d / r1m * p / r2m;

// compute initial velocity vector

for i from 1 to 3 do

    r1(i + 3) := f * r1(i) + g * r2(i);
    
end;

vwrk(1) := r1(4);

vwrk(2) := r1(5);

vwrk(3) := r1(6);

vwrk(4) := verr;

return vwrk;

end;

//////////////////////////////////////////
//////////////////////////////////////////

ziter(z, zmax, zmin, lamda, ms, nc, w, xk)

// z iteration

BEGIN

LOCAL niter := 0;

LOCAL sqrt2 := sqrt(2);

LOCAL oos2 := 1.0 / sqrt2;

LOCAL zerr := 0;

LOCAL zsaved := 2;

LOCAL w2 := w * w;

LOCAL w3 := w2 * w;

LOCAL konst := pi * ((1.0 - xk) / 2 + lamda);

LOCAL test := 1;

LOCAL vwrk := [0, 0, 0, 0];

LOCAL az, sqaz, z1, z2, z3, f1, f2;

LOCAL dn, dndz, nz;

while (test = 1) do

   az := abs(z);
   
   sqaz := sqrt(az);

   z1 := sqrt(1 - z);
   
   z2 := sqrt(1 - w2 * z);
   
   z3 := w * sqaz;

   if (nc = 1) then
   
      // elliptic orbit
      
      f1 := asin(sqaz);
      
      f2 := asin(z3);
      
   else
   
      // hyperbolic orbit
      
      f1 := ln(sqaz + sqrt(sqaz ^ 2 + 1));
      
      f2 := ln(z3 + sqrt(z3 * z3 + 1));
      
   end;

   nz := (konst + xk * (f1 - z1 * sqaz) - f2 + z2 * z3) / sqrt2 / z / sqaz;

   dndz := (xk / z1 - w3 / z2 - 3 / sqrt2 * nz) / z * oos2;

   dn := ms - nz;

   if ((abs(z - zsaved) <= 0.00000001) or (abs(dn / ms) <= 0.00000001)) then
   
      vwrk(1) := dndz;

      vwrk(2) := nz;

      vwrk(3) := z;

      vwrk(4) := zerr;

      return vwrk;
      
   end;   

   if (niter > 30) then
   
      print("niter > 30 in ziter");

      zerr := 1;
      
      break;
      
   end;

   zsaved := z;

   z := z + dn / dndz;

   if (z > zmax) then
   
      z := (zsaved + zmax) / 2;
      
   end;
   
   if (z < zmin) then
   
      z := (zsaved + zmin) / 2;
      
   end;
   
   niter := niter + 1;
   
end;

vwrk(1) := dndz;

vwrk(2) := nz;

vwrk(3) := z;

vwrk(4) := zerr;

return vwrk;

end;

//////////////////////
//////////////////////

planet(iplanet, jdate)    

// planetary ephemeris

// input

//  iplanet = planet index (1 = mercury, 2 = venus, etc.)
//  jdate   = julian day

// output

//  sv(1:3)  = heliocentric position vector (kilometers)
//  sv(4:6)  = heliocentric velocity vector (kilometers/second)
//  sv(7:12) = classical orbital elements

// Note: coordinates are with respect to the mean
//       ecliptic and equinox of date.

// Orbital Mechanics with the HP Prime

//////////////////////////////////////

BEGIN

// degrees to radians

LOCAL dtr := pi / 180.0;

LOCAL pi2 := 2.0 * pi;

LOCAL i, t, t2, t3;

LOCAL sma, ecc, inc, mlong, raan, lper;

LOCAL argper, xma, tanom;

LOCAL oev := [0, 0, 0, 0, 0, 0];

LOCAL sv := [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

// time arguments

t := (jdate - 2451545.0) / 36525.0;

t2 := t * t;

t3 := t * t2;

// evaluate orbital elements for selected planet

if (iplanet = 1) then

   // mercury 
     
   sma   :=  0.387098310;
   ecc   :=  0.20563175 + 0.000020406 * t - 0.0000000284 * t2 - 0.00000000017 * t3;
   inc   :=  7.004986 + 0.0018215 * t - 0.00001809 * t2 + 0.000000053 * t3;
   mlong :=  252.250906 + 149474.0722491 * t + 0.00030397 * t2 + 0.000000018 * t3;
   raan  :=  48.330893 + 1.1861890 * t + 0.00017587 * t2 + 0.000000211 * t3;
   lper  :=  77.456119 + 1.5564775 * t + 0.00029589 * t2 + 0.000000056 * t3;
   
end;

if (iplanet = 2) then

   // venus
   
   sma   :=  0.723329820;
   ecc   :=  0.00677188 - 0.000047766 * t + 0.0000000975 * t2 + 0.00000000044 * t3;
   inc   :=  3.394622 + 0.0010037 * t - 0.00000088 * t2 - 0.000000007 * t3;
   mlong :=  181.979801 + 58519.2130302 * t + 0.00031060 * t2 + 0.000000015 * t3;
   raan  :=  76.679920 + 0.9011190 * t + 0.00040665 * t2 - 0.000000080 * t3;
   lper  :=  131.563707 + 1.4022188 * t - 0.00107337 * t2 - 0.000005315 * t3;

end;

if (iplanet = 3) then

   // earth
   
   sma   :=  1.000001018;
   ecc   :=  0.01670862 - 0.000042037 * t - 0.0000001236 * t2 + 0.00000000004 * t3;
   inc   :=  0;
   mlong :=  100.466449 + 36000.7698231 * t + 0.00030368 * t2 + 0.000000021 * t3;
   raan  :=  0;
   lper  :=  102.937348 + 1.7195269 * t + 0.00045962 * t2 + 0.000000499 * t3;

end;

if (iplanet = 4) then

   // mars
   
   sma   :=  1.523679342;
   ecc   :=  0.09340062 + 0.000090483 * t - 0.0000000806 * t2 - 0.00000000035 * t3;
   inc   :=  1.849726 - 0.0006010 * t + 0.00001276 * t2 - 0.000000006 * t3;
   mlong :=  355.433275 + 19141.6964746 * t + 0.00031097 * t2 + 0.000000015 * t3;
   raan  :=  49.558093 + 0.7720923 * t + 0.00001605 * t2 + 0.000002325 * t3;
   lper  :=  336.060234 + 1.8410331 * t + 0.00013515 * t2 + 0.000000318 * t3;

end;

if (iplanet = 5) then

   // jupiter
   
   sma   :=  5.202603191 + 0.0000001913 * t;
   ecc   :=  0.04849485 + 0.000163244 * t - 0.0000004719 * t2 - 0.00000000197 * t3;
   inc   :=  1.303270 - 0.0054966 * t + 0.00000465 * t2 - 0.000000004 * t3;
   mlong :=  34.351484 + 3036.3027889 * t + 0.00022374 * t2 + 0.000000025 * t3;
   raan  :=  100.464441 + 1.0209550 * t + 0.00040117 * t2 + 0.000000569 * t3;
   lper  :=  14.331309 + 1.6126668 * t + 0.00103127 * t2 - 0.000004569 * t3;

end;

if (iplanet = 6) then

   // saturn
   
   sma   :=  9.554909596 - 0.0000021389 * t;
   ecc   :=  0.05550862 - 0.000346818 * t - 0.0000006456 * t2 + 0.00000000338 * t3;
   inc   :=  2.488878 - 0.0037363 * t - 0.00001516 * t2 + 0.000000089 * t3;
   mlong :=  50.077471 + 1223.5110141 * t + 0.00051952 * t2 - 0.000000003 * t3;
   raan  :=  113.665524 + 0.8770979 * t - 0.00012067 * t2 - 0.000002380 * t3;
   lper  :=  93.056787 + 1.9637694 * t2 + 0.00083757 * t2 + 0.000004899 * t3;

end;

if (iplanet = 7) then

   // uranus
   
   sma   :=  19.218446062 - 0.0000000372 * t + 0.00000000098 * t2;
   ecc   :=  0.04629590 - 0.000027337 * t + 0.0000000790 * t2 + 0.00000000025 * t3;
   inc   :=  0.773196 + 0.0007744 * t + 0.00003749 * t2 - 0.000000092 * t3;
   mlong :=  314.055005 + 429.8640561 * t + 0.00030434 * t2 + 0.000000026 * t3;
   raan  :=  74.005947 + 0.5211258 * t + 0.00133982 * t2 + 0.000018516 * t3;
   lper  :=  173.005159 + 1.4863784 * t + 0.00021450 * t2 + 0.000000433 * t3;

end;
   
if (iplanet = 8) then

   // neptune
   
   sma   :=  30.110386869 - 0.0000001663 * t + 0.00000000069 * t2;
   ecc   :=  0.00898809 + 0.000006408 * t - 0.0000000008 * t2 - 0.00000000005 * t3;
   inc   :=  1.769952 - 0.0093082 * t - 0.00000708 * t2 + 0.000000028 * t3;
   mlong :=  304.348665 + 219.8833092 * t + 0.00030926 * t2 + 0.000000018 * t3;
   raan  :=  131.784057 + 1.1022057 * t + 0.00026006 * t2 - 0.000000636 * t3;
   lper  :=  48.123691 + 1.4262677 * t + 0.00037918 * t2 - 0.000000003 * t3;
   
end;

// argument of perihelion (radians)

argper := (dtr * (lper - raan)) mod pi2;

// mean anomaly (radians)

xma := (dtr * (mlong - lper)) mod pi2;

// solve Kepler's equation for true anomaly

tanom := kepler1(xma, ecc);

// load orbital elements array

oev(1) := aunit * sma;
oev(2) := ecc;
oev(3) := dtr * inc;
oev(4) := argper;
oev(5) := (dtr * raan) mod pi2;
oev(6) := tanom;

// determine heliocentric state vector (kilometers and kilometers/second)

sv := orb2eci(oev);

// heliocentric orbital elements

for i from 1 to 6 do

    sv(i + 6) := oev(i);

end;

return sv;

end;

/////////////////////
/////////////////////

twobody2(tau, ri, vi)

// solve the two body initial value problem

// Shepperd's method

// input

//  mu  = gravitational constant (km**3/sec**2)
//  tau = propagation time interval (seconds)
//  ri  = initial eci position vector (kilometers)
//  vi  = initial eci velocity vector (kilometers/second)

// output

//  sv(1:3) = final eci position vector (kilometers)
//  sv(4:6) = final eci velocity vector (kilometers/second)

// Orbital Mechanics with the HP Prime

//////////////////////////////////////

BEGIN

LOCAL rf := [0, 0, 0];

LOCAL vf := [0, 0, 0];

LOCAL sv := [0, 0, 0, 0, 0, 0];

LOCAL r0, n0, q, n, r, l, s, d;

LOCAL gcf, k, gold, h0, h1, u0, u1, u2;

LOCAL u3, r1, dt, dtold, terror, uold, usaved;

LOCAL i, f, gg, g, ff, beta, slope, ustep;

LOCAL tolerance := 1.0e-12;

LOCAL u := 0;

LOCAL imax := 30;

LOCAL umax := 1.0e99;

LOCAL umin := -1.0e99;

LOCAL orbits := 0;

LOCAL tdesired := tau;

LOCAL threshold := tolerance * abs(tdesired);

r0 := v3mag(ri);

n0 := dot(ri, vi);

beta := 2.0 * (mu / r0) - dot(vi, vi);

if (beta <> 0) then
    
    umax := 1.0 / sqrt(abs(beta));
    
    umin := -1.0 / sqrt(abs(beta));
    
end;

if (beta > 0) then
    
    orbits := beta * tau - 2.0 * n0;
    
    orbits := 1 + (orbits * sqrt(beta)) / (pi * mu);
    
    orbits := floor(orbits / 2);
    
end;

for i from 1 to imax do

    q := beta * u * u;
    
    q := q / (1 + q);

    n :=  0;
    
    r := 1;
    
    l := 1;
    
    s := 1;
    
    d := 3;
    
    gcf := 1;
    
    k := -5;
    
    gold := 0;

    while (gcf <> gold) do

        k := -k;
        
        l := l + 2;
        
        d := d + 4 * l;
        
        n := n + (1 + k) * l;
        
        r := d / (d - n * r * q);
        
        s := (r - 1) * s;

        gold := gcf;
        
        gcf  := gold + s;

    end;

    h0 := 1 - 2 * q;
    
    h1 := 2 * u * (1 - q);

    u0 := 2 * h0 * h0 - 1;
    
    u1 := 2 * h0 * h1;
    
    u2 := 2 * h1 * h1;
    
    u3 := 2 * h1 * u2 * gcf / 3;

    if (orbits <> 0) then
        
        u3 := u3 + 2 * pi * orbits / (beta * sqrt(beta));
        
    end;

    r1 := r0 * u0 + n0 * u1 + mu * u2;
    
    dt := r0 * u1 + n0 * u2 + mu * u3;
    
    slope := 4 * r1 / (1 + beta * u * u);
    
    terror := tdesired - dt;

    if (abs(terror) < threshold) then
        
        break;
        
    end;
    
    if ((i > 1) and (u = uold)) then
        
        break;
        
    end;
    
    if ((i > 1) and (dt = dtold)) then
        
        break;
        
    end;
    
    uold  := u;
    
    dtold := dt;
    
    ustep := terror / slope;

    if (ustep > 0) then
        
        umin := u;
        
        u := u + ustep;
        
        if (u > umax) then
            
            u := (umin + umax) / 2;
            
        end;
        
    else
        
        umax := u;
        
        u := u + ustep;
        
        if (u < umin) then
            
            u := (umin + umax) / 2;
            
        end;
        
    end;

    if (i = imax) then
        
        print("max iterations in twobody2 function");
        
    end;

end;

usaved := u;

f := 1.0 - (mu / r0) * u2;

gg := 1.0 - (mu / r1) * u2;

g  :=  r0 * u1 + n0 * u2;

ff := -mu * u1 / (r0 * r1);

// final position and velocity vectors

for i from 1 to 3 do
    
    rf(i) := f * ri(i) + g  * vi(i);
    
    sv(i) := rf(i);

    vf(i) := ff * ri(i) + gg * vi(i);
    
    sv(i + 3) := vf(i);
    
end;

return sv;

END;

///////////////////
///////////////////

kepler1(manom, ecc)

// solve Kepler's equation for circular,
// elliptic and hyperbolic orbits

// Tony Danby's method

// input

//  manom = mean anomaly (radians)
//  ecc   = orbital eccentricity (non-dimensional)

// output

//  tanom = true anomaly (radians)

// Orbital Mechanics with the HP Prime

//////////////////////////////////////

BEGIN

LOCAL xma, eanom;

LOCAL s, c, f, fp, fpp, fppp, sta, cta;

LOCAL delta, deltastar, deltak, tanom;

// define convergence criterion

LOCAL ktol := 0.000000001;

LOCAL pi2 := 2.0 * pi;

xma := manom - pi2 * floor(manom / pi2);

// check for circular orbit

if (ecc = 0) then

    tanom := xma;
    
    eanom := xma;
    
    return tanom;

end;

// compute initial guess
    
if (ecc < 1) then
    
    // elliptic orbit

    eanom := xma + 0.85 * sign(sin(xma)) * ecc;

else
    
    // hyperbolic orbit

    eanom := log(2 * xma / ecc + 1.8);
    
end;

niter := 0;

// perform iterations

REPEAT

    niter := niter + 1;
    
    if (ecc < 1.0) then

        // elliptic orbit

        s := ecc * sin(eanom);

        c := ecc * cos(eanom);

        f := eanom - s - xma;

        fp := 1.0 - c;

        fpp := s;

        fppp := c;
        
    else

        // hyperbolic orbit

        s := ecc * sinh(eanom);

        c := ecc * cosh(eanom);

        f := s - eanom - xma;

        fp := c - 1.0;

        fpp := s;

        fppp := c;
        
    end;

    // update eccentric anomaly (radians)

    delta := -f / fp;

    deltastar := -f / (fp + 0.5 * delta * fpp);

    deltak := -f / (fp + 0.5 * deltastar * fpp + deltastar * deltastar * fppp / 6);

    eanom := eanom + deltak;
    
    // check for convergence

UNTIL (abs(f) <= ktol or niter > 20);

if (niter > 20) then
    
    print();
    
    print("more than 20 iterations in kepler1");
    
    wait();
    
end;

// compute true anomaly (radians)

if (ecc < 1.0) then
    
    // elliptic orbit

    sta := sqrt(1.0 - ecc * ecc) * sin(eanom);
    
    cta := cos(eanom) - ecc;
    
else
    
    // hyperbolic orbit

    sta := sqrt(ecc * ecc - 1.0) * sinh(eanom);
    
    cta := ecc - cosh(eanom);
    
end;

tanom := atan3(sta, cta);

// return true anomaly (radians)

return tanom;

END;

////////////////
////////////////

orb2eci(oev)

// convert classical orbital elements to eci 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

//  sv(1, 2, 3) = eci position vector (kilometers)
//  sv(4, 5, 6) = eci velocity vector (kilometers/second)

/////////////////////////////////////////////////////////

BEGIN

LOCAL sv := [0, 0, 0, 0, 0, 0];

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);

// semiparameter

slr := sma * (1.0 - ecc * ecc);

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

// argument of latitude (radians)

arglat := argper + tanom;

sarglat := sin(arglat);
carglat := cos(arglat);
   
c4 := sqrt(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 (kilometers)

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

// velocity vector (kilometers/second)

sv(4) := -c4 * (craan * c6 + sraan * cinc * c5);
sv(5) := -c4 * (sraan * c6 - craan * cinc * c5);
sv(6) := c4 * c5 * sinc;

return sv;

END;

//////////////
//////////////

eci2orb1(r, v)

// 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)

////////////////////////////////////////

BEGIN

LOCAL pi2 := 2.0 * pi, rtd := 180.0 / pi, rmag, vmag, hvmag;

LOCAL rhat := [0, 0, 0], hvec := [0, 0, 0], hhat := [0, 0, 0];

LOCAL ecc := [0, 0, 0], fhat := [0, 0, 0], ghat := [0, 0, 0];

LOCAL vtmp := [0, 0, 0], oev := [0, 0, 0, 0, 0, 0];

LOCAL p, q, const1, h, xk, x1, y1, xlambdat;

LOCAL sma, eccm, inc, argper, raan, tanom;

// position and velocity magnitudes

rmag := v3mag(r);

vmag := v3mag(v);

// position unit vector

rhat := r / rmag;

// angular momentum vectors

hvec := cross(r, v);

hvmag := v3mag(hvec);

hhat := hvec / hvmag;

// eccentricity vector

vtmp := v / mu;

ecc := cross(vtmp, hvec);

ecc := ecc - rhat;

// semimajor axis

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

// equinoctial orbital elements

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 := dot(ecc, ghat);

xk := dot(ecc, fhat);

x1 := dot(r, fhat);

y1 := dot(r, ghat);

// orbital eccentricity

eccm := sqrt(h * h + xk * xk);

// orbital inclination

inc := 2.0 * atan(sqrt(p * p + q * q));

// true longitude

xlambdat := atan3(y1, x1);

// check for equatorial orbit

if (inc > 0.00000001) then

    raan := atan3(p, q);

else

    raan := 0.0;

end;

// check for circular orbit

if (eccm > 0.00000001) then

    argper := (atan3(h, xk) - raan) MOD pi2;

else

    argper := 0.0;

end;

// true anomaly (radians)

tanom := (xlambdat - raan - argper) MOD pi2;

// load orbital element vector

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

// return classical orbital element vector

return oev;

END;

////////////
////////////

gdate(jdate)

// convert Julian day to Gregorian (calendar) date
   
// input

//  jdate = julian day

// output

//  cdv(1) = calendar month [1 - 12]
//  cdv(2) = calendar day [1 - 31]
//  cdv(3) = calendar year [yyyy]

// Orbital Mechanics with the HP Prime

//////////////////////////////////////

BEGIN

LOCAL jd := jdate;

LOCAL z, fday, alp, a, b, c, d, e;

LOCAL month, day, year, cdv := [0, 0, 0];

z := floor(jd + 0.5);

fday := jd + 0.5 - z;

if (fday < 0.0) then

   fday := fday + 1;

   z := z - 1;

end;

if (z < 2299161) then

   a := z;

else

   alp := floor((z - 1867216.25) / 36524.25);

   a := z + 1 + alp - floor(alp / 4);

end;
 
b := a + 1524;

c := floor((b - 122.1) / 365.25);

d := floor(365.25 * c);

e := floor((b - d) / 30.6001);

day := b - d - floor(30.6001 * e) + fday;
 
if (e < 14) then

   month := e - 1;

else

   month := e - 13;

end;
 
if (month > 2) then

   year := c - 4716;

else

   year := c - 4715;

end;

cdv(1) := month;

cdv(2) := day;

cdv(3) := year;

return cdv;

END;

////////////////////////
////////////////////////

julian(month, day, year)

// Julian date

// input

//  month = calendar month [1 - 12]
//  day   = calendar day [1 - 31]
//  year  = calendar year [yyyy]

// output

//  julian date

// special notes

//  (1) calendar year must include all digits

//  (2) calendar day may include fractional part

//  (3) will report October 5, 1582 to October 14, 1582
//      as invalid calendar date and return

// Orbital Mechanics with the HP Prime

//////////////////////////////////////

BEGIN

LOCAL y := year, m := month, a := 0, b := 0, c := 0, jd;

// check for valid calendar date

if (year == 1582 and month == 10) then

   if (day >= 5 and day <= 14) then

      MSGBOX("invalid calendar date!");

      return;

   end;

end;

if (m <= 2) then

   y := y - 1.0;

   m := m + 12.0;

end;

if (y < 0) then

   c := -0.75;

end;

if (year > 1582) then

   a := floor(y / 100);

   b := 2.0 - a + floor(a / 4);

end;

if (month > 10) then

   a := floor(y / 100);

   b := 2 - a + floor(a / 4);

end;

if (day > 14) then

   a := floor(y / 100);

   b := 2 - a + floor(a / 4);

end;

jd := floor(365.25 * y + c) + floor(30.6001 * (m + 1));
    
return jd + day + b + 1720994.5;

END;

/////////////
/////////////

jd2str(jdate)

// convert julian day to calendar
// date and time and print result

// input

//  jdate = julian date

// Orbital Mechanics with the HP Prime

//////////////////////////////////////

BEGIN

LOCAL L0 := {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sep", "Oct", "Nov", "Dec"};

LOCAL dday, ethrs, utchr, utcmin, utcsec;

LOCAL cdv := [0, 0, 0];

cdv := gdate(jdate);

print("\ncalendar date  " + L0(cdv(1)) + " " + floor(cdv(2)) + ", " + cdv(3));

dday := cdv(2);

ethrs := 24.0 * (dday - IP(dday));

utchr := IP(ethrs);
    
utcmin := IP(60.0 * (ethrs - utchr));

utcsec := 60.0 * (60.0 * (ethrs - utchr) - utcmin);

print("UTC time         " + utchr + " hr " + utcmin + " min " + utcsec + " sec ");

END;

////////////
////////////

oeprint(oev)

// print classical orbital elements

//  oev(1) = semimajor axis (au)
//  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)

////////////////////////////////////////

BEGIN

// radians-to-degrees conversion factor

LOCAL rtd := 180.0 / pi;

print(" ");

print("semimajor axis          " + oev(1) / aunit + " au");

print("eccentricity                " + oev(2));

print("inclination                  " + rtd * oev(3) + " degrees");

print("argument of perigee  " + rtd * oev(4) + " degrees");

print("raan                            " + rtd * oev(5) + " degrees");

print("true anomaly              " + rtd * oev(6) + " degrees");

END;

///////////////////
///////////////////

rvprint(reci, veci)

// print eci position and velocity vectors

// input

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

///////////////////////////////////////////

BEGIN

print(" ");

print("eci position vector (kilometers)");

print(" ");

print("r_x = " + reci(1));
print("r_y = " + reci(2));
print("r_z = " + reci(3));

print(" ");

print("eci velocity vector (kilometers/second)");

print(" ");

print("v_x = " + veci(1));
print("v_y = " + veci(2));
print("v_z = " + veci(3));

END;

///////////
///////////

svprint(sv)

// print eci state vector

// input

//  sv(1, 2, 3) = position vector (kilometers)
//  sv(4, 5, 6) = velocity vector (kilometers/second)

/////////////////////////////////////////////////////

BEGIN

print(" ");

print("eci position vector (kilometers)");

print(" ");

print("r_x = " + sv(1));
print("r_y = " + sv(2));
print("r_z = " + sv(3));

print(" ");

print("eci velocity vector (kilometers/second)");

print(" ");

print("v_x = " + sv(4));
print("v_y = " + sv(5));
print("v_z = " + sv(6));

END;

///////////
///////////

atan3(a, b)

// four quadrant inverse tangent

// input

//  a = sine of angle
//  b = cosine of angle

// output

//  0 =< angle <= 2p

////////////////////

BEGIN

LOCAL epsilon := 0.0000000001;

LOCAL pidiv2 := 0.5 * pi;

LOCAL c;

if (abs(a) < epsilon) then

    return (1.0 - sign(b)) * pidiv2;

else

    c := (2.0 - sign(a)) * pidiv2;

end;

if (abs(b) < epsilon) then

    return c;

else

    return c + sign(a) * sign(b) * (abs(atan(a / b)) - pidiv2);

end;

END;

////////
////////

v3mag(x)

// magnitude of a 3-element vector

// input

//  x = vector

//////////////

BEGIN

return sqrt(x(1)*x(1) + x(2)* x(2) + x(3) * x(3));

END;

//////////
//////////

uvector(x)

// unit vector

// input

//  x = 3-component input vector

// output

//  vhat = 3-component unit vector

// Orbital Mechanics with the HP Prime

//////////////////////////////////////

BEGIN

LOCAL vmag;

LOCAL vhat := [0, 0, 0];

vmag := sqrt(x(1) * x(1) + x(2) * x(2) + x(3) * x(3));

if (vmag > 0.0) then

   for i from 1 to 3 do
   
       vhat(i) := x(i) / vmag;
       
   end;    
   
else

   print("error in uvector");
   
   wait();
   
end;

return vhat;

end;


