path: root/src/libcf/sgp4.c

/* H+
 *      Title     : sgp4.c
 *      Author    : Bryce A. Roberts
 *      Date      : 23 February 1999
 *      Synopsis  : implementation of SGP4 propagation routines
 *      SCCS      : @(#)sgp4.c	1.6 05/14/00
 *      Revisions :
 *      mm/dd/yy name   description
 *      01/14/00 ma     get rid of unused function get_time (May 14 2000)
 *      12/18/03 bjg    Change calfusettag.h to calfuse.h 
* H-
 */

#include <string.h>
#include <time.h>
#include "calfuse.h"
#include "sgp4.h"

/* define some absolute constants used by SGP4 */
#define MIN_PER_DAY  (1440.0)
#define KM_PER_EARTH (6378.135)
#define TWOBYTHREE   (2.0/3.0)
#define Q0           (120.0)
#define S0           (78.0)
#define THREEPIBYTWO (PIBY2*3.0)
#define KE           (0.074366916)
#define AE           (1.0)
#define S            (AE*(1.0+S0/KM_PER_EARTH))
#define Q0MS4        pow((Q0-S0)*AE/KM_PER_EARTH, 4)
#define CK2          (0.5*J2*AE*AE)
#define CK4          (-0.375*J4*AE*AE*AE*AE)
#define A30          (-J3/AE*AE*AE)

#ifndef J2
#define J2           (1.082616E-3)
#endif

#ifndef J3
#define J3           (-0.253881E-5)
#endif

#ifndef J4
#define J4           (-1.65597E-6)
#endif

#ifndef PI
#define PI (3.141592653589793)
#endif

#ifndef TWOPI
#define TWOPI (2.0*PI)
#endif

#define RAD2DEG(x) (((x)*180.0)/PI)
#define DEG2RAD(x) ((PI/180.0)*(x))

#ifndef false
#define false (0)
#endif

#ifndef true
#define true (1)
#endif


/*
 * Name:     SGP4_isLeap
 * Purpose:  determine if a given year is a leap year 
 * Input:    int year (e.g. 1999)
 * Output:   returns 1 if leap year, 0 if not
 */
static int SGP4_isLeap(int year) {
  return(((year%4==0 && year%100!=0) || year%400==0));
  }


/*
 * Name:     SGP4_doy2cal
 * Purpose:  convert year/day-of-year to Gregorian calendar month and day
 * Input:    int year, doy
 * Output:   int *month, int *day, returns 0 for fail, 1 for success
 */
static int SGP4_doy2cal(int year, int doy, int *month, int *day) {
  /* number of days in leap and non-leap year months */
  static int daytab[2][13]={{0,31,28,31,30,31,30,31,31,30,31,30,31},
                            {0,31,29,31,30,31,30,31,31,30,31,30,31}};
  int leap;

  /* do range checking */
  if (doy<1 || doy>366)
    return(false);
  
  leap=SGP4_isLeap(year) ? 1 : 0;
  *day=doy;
  for (*month=1; *day>daytab[leap][*month]; (*month)++)
    *day-=daytab[leap][*month];
  
  return(true);
  }


/*
 * Name:     SGP4_create
 * Purpose:  creates an instance of SGP4
 * Input:    none
 * Output:   SGP4 
 */
SGP4 SGP4_create(void) {
  return((SGP4)calloc(1, sizeof(struct sgp4_st)));
  }


/*
 * Name:     SGP4_destroy
 * Purpose:  destroys an instance of SGP4
 * Input:    SGP4 sgp4
 * Output:   none
 */
void SGP4_destroy(SGP4 sgp4) {
  if (sgp4)
    free(sgp4);
  }


/*
 * Name:     SGP4_init
 * Purpose:  initializes time-invariant SGP4 variables
 * Input:    SGP4 sgp4
 * Output:   none
 */
void SGP4_init(SGP4 sgp4) {
  double a1, del1, a0, del0, perigee, sStar, eta_2, eta_3, eta_4, pinvsq, 
    temp1, temp2, temp3, x3thetam1, x1m5th, xhdot1, theta_4, zeta, zeta_2, 
    zeta_3, zeta_5, beta0, beta0_3, beta0_4, C2;

  /* recover original mean motion and semimajor axis from the elements */
  a1=pow(KE/sgp4->n0, TWOBYTHREE);
  del1=1.5 * (CK2/(a1*a1))*((3*cos(sgp4->i0)*cos(sgp4->i0)-1.0)/
    pow(1-sgp4->e0*sgp4->e0, 1.5));
  a0=a1*(1-del1/3.0-del1*del1-(134.0/81.0)*del1*del1*del1);
  del0=(del1*a1*a1)/(a0*a0);

  /* find original mean motion, n0dp (n0'') */
  sgp4->n0dp=sgp4->n0/(1+del0); 

  /* find semimajor axis, a0dp (a0'') */
  sgp4->a0dp=a0/(1-del0);

  /* find the perigee (in km) */
  perigee=(sgp4->a0dp*(1.0-sgp4->e0)-AE)*KM_PER_EARTH;
  
  /* make decisions on what value of s to use based on perigee */
  if (perigee<=156.0 && perigee>=98.0) {
    sStar=sgp4->a0dp*(1.0-sgp4->e0)-S-AE;
    sgp4->q0ms4=pow(pow(Q0MS4, 0.25) + S - sStar, 4);
    }
  else if (perigee<98.0) {
    sStar=20/KM_PER_EARTH + AE;
    sgp4->q0ms4=pow(pow(Q0MS4, 0.25) + S - sStar, 4);
    }
  else {
    sStar=S;
    sgp4->q0ms4=Q0MS4;
    }

  sgp4->theta=cos(sgp4->i0); sgp4->theta_2=sgp4->theta*sgp4->theta; 
  theta_4=sgp4->theta_2*sgp4->theta_2;
  zeta=1/(sgp4->a0dp-sStar);
  
  zeta_2=zeta*zeta, zeta_3=zeta_2*zeta; 
  sgp4->zeta_4=zeta_3*zeta;
  zeta_5=sgp4->zeta_4*zeta;

  sgp4->beta0_2=1-sgp4->e0*sgp4->e0;
  beta0=sqrt(sgp4->beta0_2); beta0_3=sgp4->beta0_2*beta0; 
  beta0_4=beta0_3*beta0;

  sgp4->eta=sgp4->a0dp*sgp4->e0*zeta;
  eta_2=sgp4->eta*sgp4->eta, eta_3=eta_2*sgp4->eta, eta_4=eta_3*sgp4->eta;
  
  C2=sgp4->q0ms4*(sgp4->zeta_4)*sgp4->n0dp*pow(1-eta_2, -7.0/2.0)*
    (sgp4->a0dp*(1+ ((3.0/2.0)*eta_2) + 4*sgp4->e0*sgp4->eta + 
    sgp4->e0*eta_3) + (3.0/2.0)*(CK2*zeta/(1-eta_2))*(-0.5 + 
    1.5*sgp4->theta_2)*(8.0+24*eta_2 + 3*eta_4));
  sgp4->C1=sgp4->bstar*C2; sgp4->C1_2=sgp4->C1*sgp4->C1; 
  sgp4->C1_3=sgp4->C1_2*sgp4->C1; sgp4->C1_4=sgp4->C1_3*sgp4->C1;
  sgp4->C3=(sgp4->q0ms4*(zeta_5)*A30*sgp4->n0dp*AE*sin(sgp4->i0))/
    (CK2*sgp4->e0);
  sgp4->C4=2*sgp4->n0dp*sgp4->q0ms4*(sgp4->zeta_4)*sgp4->a0dp*(sgp4->beta0_2)*
    pow(1-eta_2, -7.0/2.0)*( (2*sgp4->eta*(1+sgp4->e0*sgp4->eta)+
    0.5*sgp4->e0+0.5*eta_3)-2*CK2*zeta/(sgp4->a0dp*(1-eta_2))*
   (3*(1-3*sgp4->theta_2)*(1 + 1.5*eta_2 -2*sgp4->e0*sgp4->eta - 
   0.5*sgp4->e0*eta_3) + 0.75*(1-sgp4->theta_2)*(2*eta_2 - 
   sgp4->e0*sgp4->eta - sgp4->e0*eta_3)*cos(2*sgp4->w0)));
  sgp4->C5=2*sgp4->q0ms4*(sgp4->zeta_4)*sgp4->a0dp*(sgp4->beta0_2)*
    pow(1-eta_2, -7.0/2.0)*(1 + (11.0/4.0)*sgp4->eta*(sgp4->eta+sgp4->e0)+
    sgp4->e0*eta_3);

  sgp4->D2=4*sgp4->a0dp*zeta*sgp4->C1_2;
  sgp4->D3=(4.0/3.0)*sgp4->a0dp*zeta_2*(17*sgp4->a0dp + 
    sStar)*sgp4->C1_3;
  sgp4->D4=TWOBYTHREE*sgp4->a0dp*zeta_3*(221*sgp4->a0dp+
    31*sStar)*sgp4->C1_4;

  /* constants for secular effects calculation */
  pinvsq=1.0/(sgp4->a0dp*sgp4->a0dp*beta0_4);
  temp1=3.0*CK2*pinvsq*sgp4->n0dp;
  temp2=temp1*CK2*pinvsq;
  temp3=1.25*CK4*pinvsq*pinvsq*sgp4->n0dp;
  x3thetam1=3.0*sgp4->theta_2 - 1.0;
  x1m5th=1.0 - 5.0*sgp4->theta_2;
  xhdot1=-temp1*sgp4->theta;

  sgp4->mdot=sgp4->n0dp+0.5*temp1*beta0*x3thetam1+
    0.0625*temp2*beta0*(13.0 - 78.0*sgp4->theta_2+137.0*theta_4);

  sgp4->wdot=-0.5*temp1*x1m5th+0.0625*temp2*(7.0-114.0*sgp4->theta_2+
    395.0*theta_4)+temp3*(3.0-36.0*sgp4->theta_2+49.0*theta_4);
  
  sgp4->raandot=xhdot1+(0.5*temp2*(4.0-19.0*sgp4->theta_2)+2.0*temp3*
    (3.0-7.0*sgp4->theta_2))*sgp4->theta;
  }


/*
 * Name:     SGP4_getStateVector
 * Purpose:  finds position, velocity at a time
 * Input:    SGP4 sgp4, double t
 * Output:   double pos[3], double vel[3]
 */
void SGP4_getStateVector(SGP4 sgp4, double t, double pos[3], double vel[3]) {
  double mdf, wdf, raandf, del_w, del_m, mp, w, RAAN, e, a, IL,
    beta_2, n, axn, ILl, aynl, ILt, ayn, U, initial, eCosE,
    eSinE, eL_2, pL, r, rDot, rfDot, cosu, sinu, u, rk, uk, RAANk, ik,
    rDotk, rfDotk, mx, my, ux, uy, uz, vx, vy, vz, tsince;
  int i;

  /* find the time since the epoch, in minutes */
  tsince=(t-sgp4->epochTime)*1440;

  /* secular effects of atmospheric drag and gravitation */
  mdf=sgp4->M0+(sgp4->mdot*tsince);
  wdf=sgp4->w0+(sgp4->wdot*tsince);
  raandf=sgp4->raan+(sgp4->raandot*tsince);
  
  del_w=sgp4->bstar*sgp4->C3*(cos(sgp4->w0))*tsince;
  del_m=-TWOBYTHREE*sgp4->q0ms4*sgp4->bstar*sgp4->zeta_4*
    (AE/(sgp4->e0*sgp4->eta))*(pow(1+sgp4->eta*cos(mdf), 3)-
    pow(1+sgp4->eta*cos(sgp4->M0), 3));
  
  mp=mdf+del_w+del_m;
  w=wdf-del_w-del_m;
  
  RAAN=raandf-(21.0/2.0)*((sgp4->n0dp*CK2*sgp4->theta)/(sgp4->a0dp*
    sgp4->a0dp*sgp4->beta0_2))* sgp4->C1*tsince*tsince;
  e=sgp4->e0-sgp4->bstar*sgp4->C4*tsince-sgp4->bstar*sgp4->C5*
    (sin(mp)-sin(sgp4->M0));
  
  a=sgp4->a0dp*pow((1+tsince*(-sgp4->C1+tsince*(-sgp4->D2+tsince*
    (-sgp4->D3-sgp4->D4*tsince)))), 2);
  
  IL=mp+w+RAAN+sgp4->n0dp*tsince*tsince*(1.5*sgp4->C1+tsince*
    ((sgp4->D2+2*sgp4->C1_2) + tsince*(0.25*(3*sgp4->D3 + 
    12*sgp4->C1*sgp4->D2 + 10*sgp4->C1_3)+tsince*(0.2*(3*sgp4->D4+12*
    sgp4->C1*sgp4->D3+6*sgp4->D2*sgp4->D2+30*sgp4->C1_2*sgp4->D2+
    15*sgp4->C1_4)))));

  beta_2=1-e*e;
  n=KE/pow(a, 3.0/2.0);

  /* find the long-period periodic terms */
  axn=e*cos(w);
  ILl=(A30*sin(sgp4->i0))/(8*CK2*a*beta_2)*(e*cos(w))*((3+5*sgp4->theta)/
    (1+sgp4->theta));
  aynl=(A30*sin(sgp4->i0))/(4*CK2*a*beta_2);
  ILt=IL+ILl;
  ayn=e*sin(w)+aynl;
    
  /* iteratively solve Kepler's equation */
  U=fmod(ILt-RAAN, TWOPI);

  initial=U;
  for (i=0; i<10; i++) {
    U=(initial-ayn*cos(U)+axn*sin(U)-U)/(1.0-ayn*sin(U)-axn*cos(U))+U;
    }

  /* preliminary quantities for short period periodics */
  eCosE=axn*cos(U)+ayn*sin(U); 
  eSinE=axn*sin(U)-ayn*cos(U);
  eL_2=axn*axn+ayn*ayn;
  pL=a*(1-eL_2);
  r=a*(1-eCosE);
  rDot=KE*(sqrt(a)/r)*eSinE;
  rfDot=KE*sqrt(pL)/r;
  cosu=(a/r)*(cos(U)-axn+(ayn*eSinE)/(1+sqrt(1-eL_2)));
  sinu=(a/r)*(sin(U)-ayn-(axn*eSinE)/(1+sqrt(1-eL_2)));
  u=fmod(atan2(sinu, cosu)+TWOPI, TWOPI);

  /* update for short-period periodics */
  rk=r*(1-1.5*CK2*sqrt(1-eL_2)/(pL*pL)*(3*sgp4->theta_2-1))+
    (CK2/(2*pL))*(1-sgp4->theta_2)*cos(2*u);
  uk=u+-(CK2/(4*pL*pL))*(7*sgp4->theta_2-1.0)*sin(2*u);;
  RAANk=RAAN+((3*CK2*sgp4->theta)/(2*pL*pL))*sin(2*u);
  ik=sgp4->i0+((3*CK2*sgp4->theta)/(2*pL*pL))*sin(sgp4->i0)*cos(2*u);
  rDotk=rDot-((CK2*n)/pL)*(1-sgp4->theta_2)*sin(2*u);
  rfDotk=rfDot+((CK2*n)/pL)*((1-sgp4->theta_2)*cos(2*u)-
  1.5*(1-3*sgp4->theta_2));
  
  /* find the position and velocity components */
  mx=-sin(RAANk)*cos(ik);
  my=cos(RAANk)*cos(ik);
  
  ux=mx*sin(uk)+cos(RAANk)*cos(uk);
  uy=my*sin(uk)+sin(RAANk)*cos(uk);
  uz=sin(ik)*sin(uk);
  
  vx=mx*cos(uk)-cos(RAANk)*sin(uk);
  vy=my*cos(uk)-sin(RAANk)*sin(uk);
  vz=sin(ik)*cos(uk);

  pos[0]=KM_PER_EARTH*rk*ux;
  pos[1]=KM_PER_EARTH*rk*uy;
  pos[2]=KM_PER_EARTH*rk*uz;

  vel[0]=MIN_PER_DAY*KM_PER_EARTH*(rDotk*ux+rfDotk*vx)/(AE*86400.0);
  vel[1]=MIN_PER_DAY*KM_PER_EARTH*(rDotk*uy+rfDotk*vy)/(AE*86400.0);
  vel[2]=MIN_PER_DAY*KM_PER_EARTH*(rDotk*uz+rfDotk*vz)/(AE*86400.0);
  }


/*
 * Name:     SGP4_set
 * Purpose:  sets the elements of an instance of SGP4
 * Input:    SGP4 sgp4, double epochTime, double n0dt, double n0dt2,
 *             double bstar, double i0, double raan, double e0,
 *             double w0, double M0, double n0
 * Output:   none
 */
void SGP4_set(SGP4 sgp4, double epochTime, double n0dt, double n0dt2,
	      double bstar, double i0, double raan, double e0,
	      double w0, double M0, double n0) {

  /* set the epoch time */
  sgp4->epochTime=epochTime;

  /* set the first time derivative of mean motion */
  sgp4->n0dt=n0dt*(TWOPI/(MIN_PER_DAY*MIN_PER_DAY));

  /* set the second time derivative of mean motion */
  sgp4->n0dt2=n0dt2*(TWOPI/(MIN_PER_DAY*MIN_PER_DAY));
      
  /* bstar drag term */
  sgp4->bstar=bstar;

  /* inclination */
  sgp4->i0=DEG2RAD(i0);
      
  /* right ascension of the ascending node */
  sgp4->raan=DEG2RAD(raan);

  /* eccentricity */
  sgp4->e0=e0;

  /* argument of perigee  */
  sgp4->w0=DEG2RAD(w0);

  /* mean anomaly */
  sgp4->M0=DEG2RAD(M0);

  /* mean motion */
  sgp4->n0=n0*(TWOPI/MIN_PER_DAY);
  }


/*
 * Name:     SGP4_getPole
 * Purpose:  finds the pole (orbit plane normal)
 * Input:    double pos[3], double vel[3]
 * Output:   double pole[3]
 */
void SGP4_getPole(double pos[3], double vel[3], double pole[3]) {
  register double pNorm;

  /* find the cross product between position and velocity */
  pole[0]=pos[1]*vel[2] - vel[1]*pos[2];
  pole[1]=pos[2]*vel[0] - vel[2]*pos[0];
  pole[2]=pos[0]*vel[1] - vel[0]*pos[1];

  /* normalize the resulting pole vector */
  pNorm=sqrt(pole[0]*pole[0]+pole[1]*pole[1]+pole[2]*pole[2]);
  pole[0]/=pNorm; pole[1]/=pNorm; pole[2]/=pNorm;
  }


/*
 * Name:     SGP4_precess
 * Purpose:  precesses a direction vector from t1 to t2
 * Input:    double v[3], double t1, double t2
 * Output:   double v[3]
 */
void SGP4_precess(double v[3], double t1, double t2) {
  double t, st, a, b, c, temp[3], sina, sinb, sinc, cosa, cosb, cosc;

  /* convert times to years */
  t=0.001*(t2-t1)/365.25;
  st=0.001*(t1-MJD2000)/365.25;

  /* find the Euler angles */
  a=DEG2RAD(t*(23062.181+st*(139.656+0.0139*st)+
	       +t*(30.188-0.344*st+17.998*t)))/3600.0;
  b=DEG2RAD(t*t*(79.280+0.410*st+0.205*t)/3600.0)+a;
  c=DEG2RAD(t*(20043.109-st*(85.33+0.217*st)+
	       +t*(-42.665-0.217*st-41.833*t)))/3600.0;
  
  /* do the precession rotation */
  sina=sin(a); sinb=sin(b); sinc=sin(c);
  cosa=cos(a); cosb=cos(b); cosc=cos(c);
  temp[0]=(cosa*cosb*cosc-sina*sinb)*v[0]+(-cosa*sinb-sina*cosb*cosc)*v[1]+
    -cosb*sinc*v[2];
  temp[1]=(sina*cosb+cosa*sinb*cosc)*v[0]+(cosa*cosb-sina*sinb*cosc)*v[1]+
    -sinb*sinc*v[2];
  temp[2]=cosa*sinc*v[0]-sina*sinc*v[1]+cosc*v[2];
  
  /* replace existing direction vector */
  v[0]=temp[0]; v[1]=temp[1]; v[2]=temp[2];
  }