View source for Astro Mechanics


TGeoid
 <nowiki>
//---------------------------------------------------------------------------
//  Class Name:   TGeoid
//
//  Description: Encapsulates Geodetic Ellipsoid Coordinate Transformation
//               Calculations.
//
//  Written By:  R. James Lanzi
//
//  Notes:
//
//  Modification History:
//  4/24/00  R.J.L. Encapsulated Geodetic Subroutine Library in TGeoid Class
//  4/16/02  Modified Gravitational Model from GEM-model to J4-Model
//  8/14/02  Modified Stand-Off Buffer calculation to include Growth Factor
//           Capability
//  10/18/02 Stripped Stand-Off Buffer calculation form TGeoid Base Class -
//           encapsulated these routines in a new TGeoBoundary child class.
//  4/24/03  Added EFG2TxConstants function to permit more efficient use of
//           computed lat, long, and altitude. Use instead of EFG2Teg when
//           having geodetic coordinates is desired.
//  12/10/03 Added Time To Impact and Time To Apogee as a Return Arguments of
//            EFG2Kepler
//  1/9/04   Modified EFG2Kepler to return negative Time To Apogee if descending
//  3/8/04   Consolidated argument checking code into safeAtan2 and safeAcos2
//           functions. Converted all acos and atan2 functions into safe
//           versions.
//  3/8/04   Modified algorithm to iterate for Eccentric anomaly at epoch
//           (converges faster). Also modified comment to correctly reflect
//           output of true anomaly at epoch in EFG2Kepler.
//  3/8/04   Added Lots and Lots of Comments to support upcoming code review.
//  3/17/04  RA2ll modifications - Modified basis vector formation algorithm.
//           Strengthened convergence criterion.
//  3/17/04  Improved protection against floating point exceptions in RA2ll,
//           EFG2llh, EFG2Kepler, EFG2Gravity
//  9/13/04  Implemented Recommendations of Code Review held on 7/27/04
//            - Added Comments.
//            - Added data access for J2, J3, & J4
//            - Updated J2,J3,J4 default parameters-directly trace to WGS84 EGM
//            - Added temporary variables when working in different unit systems
//            - Modified EFGv2llhv to call EFG2TxConstants to minimize code
//              segment duplication
//            - Added Error Status Return value to all Kepler Returns.
//              Included addition of enumerated data type: TGeoid::EKeplerStatus
//            - Modified EFG2Gravity to eliminate trig and pow() calls
//  4/14/05  Implemented safeguards against divide-by-zero and sqrt argument
//           faults in EFG2Kepler routine. (Vulnerability identified for perfectly
//           circular orbits).
//
//---------------------------------------------------------------------------
#ifndef TGeoidH
#define TGeoidH
class TGeoid {
   private:
      double a,                     // Equatorial Radius
             invf,                  // Inverse Flattening
             wE,                    // Earth Rotation Rate
             mu;                    // Geocentric Gravitational Constant
      double J2,J3,J4;              // Harmonic Gravitational Constants
      double b;                     // Polar Radius
      int    kMaxIterate;           // Used to cap number of times to iterate
      double kDenomEps;             // Denominator Epsilon for DivByZero Checks
      double degToRad;              // Degrees to Radians Conversion
      double radToDeg;              // Radians to Degress Conversion

   public:
     // Enumerated Error Status returned from all Kepler functions
     enum  EKeplerStatus {ksNormalExit, ksZeroRadius, ksZeroAngMom,
                          ksNonEllipticalOrbit};


   public:
// Default Constructor uses WGS-84 Earth Model Constants
      TGeoid(double = 6378137.000,    // WGS84 Equatorial Radius (m)
             double = 298.257223563,  // WGS84 Inverse Flattening
             double = 7.292115e-5,    // WGS84 Rotation Rate of Earth (rad/sec)
             double = 398600.5,       // WGS84 Gravitational Constant (km^3/s^2)
             double = 1.08263E-3,     // J2 (derived from WGS84 C20)
             double = -2.53215307E-6, // J3 (derived from WGS84 C30)
             double = -1.61098761E-6); //J4 (derived from WGS84 C40)
//
//    Model Constant Data Access Functions
//
      double getEqRad(void)   {return a;};
      double getInvFlat(void) {return invf;};
      double getPoleRad(void) {return b;};
      double getOmega(void)   {return wE;};
      double getMu(void)      {return mu;};
      double getJ2(void)      {return J2;};
      double getJ3(void)      {return J3;};
      double getJ4(void)      {return J4;};

      void   setEqRad(double _a);
      void   setInvFlat(double _invf);
      void   setPoleRad(double _b);
      void   setOmega(double _wE) {wE = _wE;};
      void   setMu(double _mu)    {mu = _mu;};
      void   setJ2(double _J2)    {J2 = _J2;};
      void   setJ3(double _J3)    {J3 = _J3;};
      void   setJ4(double _J4)    {J4 = _J4;};
      void   setMaxIterateCount(int _kMaxIterate) {kMaxIterate=_kMaxIterate;};

// Earth Radius Functions
      double getLatCRad(double latC);
      double getLatGRad(double latG);

//
//    Geodetic Coordinate Transformation and Inverse Transformation Functions
//

//  Latitude,Longitude <--> Range, Azimuth

      void ll2RA(double lat0, double lon0, double lat1, double lon1,
           double &Range, double &Azimuth );
      void RA2ll(double lat0, double lon0, double Range, double Azimuth,
           double &lat1, double &lon1 );

//  Latitude,Longitude,Altitude <--> E,F,G Cartesian

      void llh2EFG(double lat, double lon,double alt, double *EFG);
      void EFG2llh(double *EFG, double &lat, double &lon, double &alt);
      void llh2ECI(double time, double le0, double lat, double lon, double alt,
                                 double *ECI);

//  Lat., Long., Alt. <--> Cartesion + Geodetic Frame Direction Cosines

      void llh2TxConstants(double latitude, double longitude, double altitude,
		     double *EFGSite,double *TegSite);
      void EFG2TxConstants(double *EFGSite,double *TegSite, double &latitude,
                           double &longitude, double &altitude);
      void EFG2Teg(double *EFGSite,double *TegSite);

//  Latitude,Longitude,Altitude,Vel,FltEl,FltAz <--> E,F,G,Edot,Fdot,Gdot

       void llhv2EFGv(double lat,double lon,double alt,
               double vel,double fltel,double fltaz,
               double *EFG,double *VEFG);
       void EFGv2llhv(double *EFG, double *VEFG,
               double &latitude, double &longitude, double &altitude,
               double &Velocity, double &FltEl, double &FltAz);
       void llhv2ECIv(double Time, double LE0, double latitude,
               double longitude,double altitude,
               double vel,double fltel,double fltaz,
               double *ECI,double *VECI);
       void ECIv2llhv(double time, double LE0, double *ECI, double *VECI,
               double &latitude, double &longitude, double &altitude,
               double &Velocity, double &FltEl, double &FltAz);


//  Latitude,Longitude,Altitude  <--> Slt Range, Look Az, Look El

      void llh2RAE(double lat0, double lon0, double alt0,
               double lat1, double lon1, double alt1,
               double &Range, double &Azimuth, double &Elevation );
      void RAE2llh(double lat0, double lon0, double alt0,
               double Range, double Azimuth, double Elevation ,
               double &lat1, double &lon1, double &alt1);
      void RAE2EFG(double Range, double Azimuth, double Elevation,
	       double *EFGSite, double *TegSite, double *EFG);

//  E,F,G,Edot,Fdot,Gdot <--> Keplerian Orbital Elements

      EKeplerStatus EFG2Kepler(double GHA0, double t0,
               double *EFG0, double *VEFG0, double ImpactTol,
               double &a, double &p, double &ecc, double &inclination,
               double &apogee, double &perigee, double &nu, double &nu0,
               double &LonNode, double &ArgPeri,
               double &LatImpact, double &LonImpact,
               double &TimeToImpact, double &TimeToApogee,
               int &OrbitFlag) ;

      // Included for Backward Compatibility - Forwards call to current version
      EKeplerStatus EFG2Kepler(double GHA0, double t0,
               double *EFG0, double *VEFG0, double ImpactTol,
               double &a, double &p, double &ecc, double &inclination,
               double &apogee, double &perigee, double &nu, double &nu0,
               double &LonNode, double &ArgPeri,
               double &LatImpact, double &LonImpact,
               int &OrbitFlag) ;

      EKeplerStatus Kepler2EFG(double GHA0, double t, double a, double ecc, double nu0,
               double Inclination, double LonNode, double ArgPeri,
               double &nu, double *EFG, double *VEFG);


// Gravity/Coriolis Routines
      void EFG2Gravity(double *EFG, double *gE);
      void EFGv2RotAcc(double *x, double *v, double *arot);

//  Private Utility Functions
   protected:
      double sign(double x) {return x<0.0 ? -1.0 : 1.0;};
      void   cross(double *firstVector, double *secondVector,double *outVector);
      double dot(double *aVector, double *anotherVector);
      void   matv(double *aMatrix, double *aVector,
                  double *outVector,const int doTranspose);
      double norm2(double *aVector);
   public:
      double safeAtan2(double y, double x);
      double safeAcos(double x);
};
#endif
</nowiki>

 <nowiki>
#include "TGeoid.h"
#include <math.h>

//
// ... Define PI constant if it is not already done
//
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

//-----------------------------------------------------------------------
// Function: TGeoid
//
// Purpose: Constructor Initializes Geoid Constants
//
//-----------------------------------------------------------------------
TGeoid::TGeoid(double _a, double _invf, double _wE, double _mu, double _J2,
               double _J3, double _J4) {
   a = _a;
   invf = _invf;
   b = a - a/invf;
   wE = _wE;
   mu = _mu;
   J2 = _J2;
   J3 = _J3;
   J4 = _J4;
   kMaxIterate = 30;    // May or May not be suitable for Real-Time application
                        // Can be as low as 5 and produce satisfactory results
   kDenomEps = 1.e-9;

   // Global Conversion Factors
   degToRad = M_PI/180.0;
   radToDeg = 180.0/M_PI;
}
//-----------------------------------------------------------------------
// Function: setEqRad
//
// Purpose: Data Access Function for setting Equatorial Radius
//
//-----------------------------------------------------------------------
void TGeoid::setEqRad(double _a) {
   a = _a;
   b = a - a/invf;
}

//-----------------------------------------------------------------------
// Function: setInvFlat
//
// Purpose: Data Access Function for setting Inverse Flattening Constant
//
//-----------------------------------------------------------------------
void TGeoid::setInvFlat(double _invf) {
   invf = _invf;
   b = a - a/invf;
}

//-----------------------------------------------------------------------
// Function: setPoleRad
//
// Purpose: Data Access Function for setting Polar Radius
//
//-----------------------------------------------------------------------
void TGeoid::setPoleRad(double _b) {
   b = _b;
   invf = a/(a-b);
}

//-----------------------------------------------------------------------
// Function: getLatCRad
//
// Purpose: Fetch Geoid Radius at a Geocentric Latitude
//-----------------------------------------------------------------------
double TGeoid::getLatCRad(double LatC) {
   double s,c;
   s = sin(LatC*degToRad);
   c = cos(LatC*degToRad);
   return b/sqrt(s*s+(b/a)*(b/a)*c*c);
}
//-----------------------------------------------------------------------
// Function: getLatGRad
//
// Purpose: Fetch Geoid Radius at a Geodetic Latitude
//-----------------------------------------------------------------------
double TGeoid::getLatGRad(double LatG) {
   double s,c;
   s = sin(LatG*degToRad);
   c = cos(LatG*degToRad);
   return a*sqrt(c*c+(b/a)*(b/a)*(b/a)*(b/a)*s*s)/sqrt(c*c+(b/a)*(b/a)*s*s);
}

//-----------------------------------------------------------------------
// Function: ll2RA
//
// Purpose: Oblate Range/Azimuth Calculation from lat/lon coordinates
//
// Input Arguments:
//   lat0,lon0       Reference Geodetic latitude and longitude (deg)
//   lat1,lon1       Target Geodetic latitude and longitude (deg)
//
// Output Arguments (passed by reference):
//   Range, Azimuth    Range (m) and Azimuth (deg) from Reference to Target
//                    over oblate earth
//-----------------------------------------------------------------------
void TGeoid::ll2RA(double lat0, double lon0, double lat1, double lon1,
           double &Range, double &Azimuth ) {
   int i;
   double Teg0[9],EFG0[3],EFG1[3],R0,R1;
   double dR[3],dRg[3],phi;

   // Start Point ECEF (3d) coordinates and direction cosines of geodetic frame
   llh2TxConstants(lat0, lon0, 0.0, EFG0, Teg0);

   // Finish point ECEF (3d) coordinates
   llh2EFG(lat1, lon1, 0.0, EFG1);

   // Earth Radii at Start and Finish Points
   R0 = norm2(EFG0);
   R1 = norm2(EFG1);

   phi = 0.0;
   for(i = 0; i<3; i++) {
      // Look Vector from Start Point to Finish Point
      dR[i] = EFG1[i] - EFG0[i];

      // Dot product of Earth Centered Unit Vectors for Start and Finish Points
      phi = phi + (EFG0[i]/R0)*(EFG1[i]/R1);
   }

   // Great Circle Angle between Start and Finish Points
   phi = safeAcos(phi);

   // Use Average Earth Radius for two points to compute a spherical earth range
   Range = 0.5*(R0+R1)*phi;

   // Compute components of look vector in Pt0 local North-East-Down frame
   matv(Teg0,dR,dRg,1);

   // Compute Azimuth from Pt0 local horizontal projection of look vector
   Azimuth = safeAtan2(dRg[1],dRg[0])*radToDeg;
   if(Azimuth < 0.0) Azimuth += 360.0;

}
//-----------------------------------------------------------------------
// Function: RA2ll
//
// Purpose: Oblate lat/lon Calculation from Range/Azimuth coordinates
//
// Input Arguments:
//   lat0,lon0       Reference Geodetic latitude and longitude (deg)
//   Range, Azimuth  Range (m) and Azimuth (deg) from Reference to Target
//                    over oblate earth
//
// Output Arguments:   (Passed by reference)
//   lat1,lon1       Target Geodetic latitude and longitude (deg)
//
// NOTE: This function suffers from geometrically induced numerical ill
// conditioning for Range values that produce great-circle arc angles near
// 180-deg. The algorithm will function for such inputs; however, the
// iterations will last longer, and the results will not be perfectly invertible
// with the ll2RA function.
//
//-----------------------------------------------------------------------
void TGeoid::RA2ll(double lat0, double lon0, double Range, double Azimuth,
           double &lat1, double &lon1 ) {
   int Iterate;
   double Teg0[9],EFG0[3],EFG1[3],R0,R1,alt1,Ravg,Elevation,deltaEl;
   double phi;
   double deltaR;

   // Start Point ECEF coodinates and direction cosines of local geodetic frame
   llh2TxConstants(lat0,lon0,0.0,EFG0,Teg0);

   // Earth Radius at Start Point
   R0 = norm2(EFG0);

   // Initialize Guesses for Average Earth Radius and Elevation Bias
   R1 = R0;
   deltaEl = 0.0;
   Iterate = 0;
   do {
      // Compute Average Earth Radius based Upon Finish Point Radius Guess
      Ravg = 0.5*(R0+R1);

      // Great circle angle between start and finish point
      phi = Range/Ravg;

      // Straight line distance from start to finish point
      deltaR = R0*R0 + R1*R1 - 2.0*R0*R1*cos(phi);

      if(deltaR >= 0.0) {
         deltaR = sqrt(deltaR);
      }
      else {
         // This condition can only happen from poor numerical conditioning
         // around phi = 0
         deltaR = 0.0;
      }

      // Best guess at straight line look elevation from start to finish
      Elevation = (-phi/2.0 + deltaEl)*radToDeg;

      // Shoot to finish point estimate
      RAE2EFG(deltaR, Azimuth, Elevation, EFG0, Teg0, EFG1);

      // Compute finish point estimate lat long and altitude
      EFG2llh(EFG1,lat1,lon1,alt1);

      // Adjust Finish Point Radius to be used in Great Circle Angle Calculation
      R1 = getLatGRad(lat1);

      // Adjust Elevation Bias
      if(fabs(deltaR) > kDenomEps) {
         deltaEl += -cos(phi/2.0)*alt1/deltaR;
      }

      Iterate++;

      // Algorithm has converged when finish point lands on ellipsoid
   } while (fabs(alt1)>1.e-5 && Iterate < kMaxIterate);
}
//-----------------------------------------------------------------------
// Function: llh2EFG
//
// Purpose: Convert from Latitude, Longitude, and Altitude
//          to Earth Fixed Geocentric (EFG) X,Y, and Z coordinates
//
// Input:
//   latitude          Geodetic latitude (deg)
//   longitude         Longitude (deg)
//   altitude          Height above ellipsoid (m)
//
// Output: (Passed by pointer)
//   EFG[3]            E, F, and G coordinates in meters
//
//-----------------------------------------------------------------------
void TGeoid::llh2EFG(double latitude, double longitude, double altitude,
                     double *EFG)
{
   double sph,cph,slm,clm,tmp,X;

   // Do Trig Once.
   sph = sin(latitude*degToRad);
   cph = cos(latitude*degToRad);
   slm = sin(longitude*degToRad);
   clm = cos(longitude*degToRad);

   // Intermediate Values
   tmp = sqrt((b/a)*(b/a)*sph*sph+cph*cph);
   X = a*cph/tmp + altitude*cph;


   EFG[0] = X*clm;
   EFG[1] = X*slm;
   EFG[2] = b/a*b*sph/tmp + altitude*sph;
}
//-----------------------------------------------------------------------
// Function: EFG2llh
//
// Purpose: Convert from EFG (Earth Fixed Geocentric) Coordinates to
// Latitude, Longitude, Altitude
//
// Input Argument:
//   EFG[3]         E, F, and G coordinates in meters
//
// Output:   (Passed by reference)
//   latitude       Geodetic latitude (deg)
//   longitude      Longitude (deg)
//   altitude       Height above ellipsoid (m)
//-----------------------------------------------------------------------
void TGeoid::EFG2llh(double *EFG, double &latitude, double &longitude,
                                                    double &altitude)
{
   double lat_c,rI,u,uold,sph,cph,Xs,Zs,tmp,Dx,Dz,latRadians;
   int i;

   // Pos Vector Length
   rI = norm2(EFG);

   // Protect Against Floating Point Exceptions stemming from rI = 0
   if(rI < kDenomEps) {
      latitude = 0.0;
      longitude = 0.0;
      altitude = -getEqRad();
      return;
   }

   // Geocentric Latitude
   lat_c = safeAtan2(EFG[2],sqrt(EFG[0]*EFG[0]+EFG[1]*EFG[1]));

   longitude = safeAtan2(EFG[1],EFG[0])*radToDeg;

   sph = sin(lat_c);
   cph = cos(lat_c);

   // Iteration for Geodetic Latitude
   // Source: GEM User's Manual, Pg. A-9 & A-10
   u = 1.0;
   for(i = 0; i<kMaxIterate; i++) {
      uold = u;
      u = b/a + (a-b/a*b)*u/rI/sqrt(cph*cph+u*u*sph*sph);
      // Stop iterating when we have done the best we can
      if(u == uold) break;
   }
   latRadians = safeAtan2(u*a/b*sph,cph);

   sph = sin(latRadians);
   cph = cos(latRadians);

   latitude = latRadians*radToDeg;

   //  Surface 'X' and Z Coordinates
   tmp = sqrt((b/a)*(b/a)*sph*sph+cph*cph);
   Xs = a*cph/tmp;
   Zs = b/a*b*sph/tmp;

   // In-Plane Components of Vector from Sub-Point to Point of Interest
   Dx = sqrt(EFG[0]*EFG[0]+EFG[1]*EFG[1]) - Xs;
   Dz = EFG[2] - Zs;

   // Altitude:
   //   Magnitude = Magniude of Subpoint to Vehicle Vector
   //   Sign based upon dot product of subpoint to Vehicle vector w/local Normal
   altitude = sign(Dx*Xs+Dz*Zs)*sqrt(Dx*Dx + Dz*Dz);

}
//-----------------------------------------------------------------------
// Function: llhv2EFGv
//
// Purpose: Position and velocity coordinate transformations
//          Geodetic to ECEF Cartesian
//
// Input:
//   latitude           Geodetic latitude (deg)
//   longitude          Longitude (deg)
//   altitude           Height above ellipsoid (m)
//   vel                Velocity (m/s)
//   fltel              Velocity vector elevation (deg)
//   fltaz              Velocity vector azimuth (deg)
//
// Output: (Passed by Pointer)
//   EFG[3]            E, F, and G coordinates in meters
//   VEFG[3]           Edot, Fdot, Gdot (m/s)
//
//-----------------------------------------------------------------------
void TGeoid::llhv2EFGv(double latitude,double longitude,double altitude,
               double vel,double fltel,double fltaz,
               double *EFG,double *VEFG)
{
   double Teg[9],Vg[3],ce;

   // Compute ECEF position coordinates and direction cosines of local geodetic
   llh2TxConstants(latitude, longitude, altitude,EFG,Teg);

   // Compute velocity components in local geodetic (NED) frame
   ce = cos(fltel*degToRad);
   Vg[0] = vel*ce*cos(fltaz*degToRad);
   Vg[1] = vel*ce*sin(fltaz*degToRad);
   Vg[2] = -vel*sin(fltel*degToRad);

   // Transform to ECEF components
   matv(Teg,Vg,VEFG,0);
}
//-----------------------------------------------------------------------
// Function: llh2ECI
//
// Purpose: Position coordinate transformations
//          Geodetic to Earth Centered Inertial Cartesian
//
// Input:
//   Time                Elapsed Time since Epoch (sec)
//   LE0                 Greenwich Longitude at Epoch (deg)
//   latitude           Geodetic latitude (deg)
//   longitude          Longitude (deg)
//   altitude           Height above ellipsoid (m)
//
// Output:
//   ECI[3]              E, F, and G coordinates in meters
//
//-----------------------------------------------------------------------
void TGeoid::llh2ECI(double Time, double LE0, double latitude,
               double longitude,double altitude,
               double *ECI)
{
   double LongI;
   // Inertial Longitude = Start Position of Earth + Earth Rotation + Longitude
   LongI = LE0 + wE*Time*radToDeg + longitude;

   // Can use ECEF Library Routine to do Coordinate Transformation
   llh2EFG(latitude, LongI, altitude, ECI);
}
//-----------------------------------------------------------------------
// Function: llhv2ECIv
//
// Purpose: Position and velocity coordinate transformations
//          Geodetic to Earth Centered Inertial Cartesian
//
// Input:
//   Time               Elapsed Time since Epoch (sec)
//   LE0                Greenwich Longitude at Epoch (deg)
//   latitude           Geodetic latitude (deg)
//   longitude          Longitude (deg)
//   altitude           Height above ellipsoid (m)
//   vel                Velocity (m/s)
//   fltel              Velocity vector elevation (deg)
//   fltaz              Velocity vector azimuth (deg)
//
// Output:
//   ECI[3]             E, F, and G coordinates in meters
//   VECI[3]            Edot, Fdot, Gdot (m/s)
//
//-----------------------------------------------------------------------
void TGeoid::llhv2ECIv(double Time, double LE0, double latitude,
               double longitude,double altitude,
               double vel,double fltel,double fltaz,
               double *ECI,double *VECI)
{
   double Teg[9],Vg[3],ce,LongI;

   // Inertial Longitude = Start Position of Earth + Earth Rotation + Longitude
   LongI = LE0 + wE*Time*radToDeg + longitude;

   // Compute Inertial Coordinates and Direction Cosines of 'Inertial Geodetic'
   llh2TxConstants(latitude, LongI, altitude, ECI, Teg);

   // Compute Components of Earth Relative Velocity in Inertial Geodetic Frame
   ce = cos(fltel*degToRad);
   Vg[0] = vel*ce*cos(fltaz*degToRad);
   Vg[1] = vel*ce*sin(fltaz*degToRad);
   Vg[2] = -vel*sin(fltel*degToRad);

   // Transform from Geodetic to Earth Centered Inertial Components
   matv(Teg,Vg,VECI,0);

   // Add Earths Rotation to form Inertial Velocity
   VECI[0] = VECI[0] - wE*ECI[1];
   VECI[1] = VECI[1] + wE*ECI[0];
}
//-----------------------------------------------------------------------
// Function: ECIv2llhv
//
// Purpose: Convert from Earth Centered Inertial cartesian components of
//          position and velocity to local geodetic position and velocity
//
// Input:
//   time                Elapsed Time since Epoch (sec)
//   LE0                 Greenwich Longitude at Epoch (deg)
//   ECI[3]              XI,YI,ZI (m)
//   VECI[3]             X,Y,Z Inertial Velocity (m/s)
//
// Output:   (Passed by reference)
//   latitude             Geodetic latitude (deg)
//   longitude            Longitude (deg)
//   altitude             Height above ellipsoid (m)
//   Velocity             Magnitude (m/s)
//   FltAz                Flight Azimuth (deg)
//   FltEl                Flight Elevation (deg)
//-----------------------------------------------------------------------
void TGeoid::ECIv2llhv(double time, double LE0, double *ECI, double *VECI,
double &latitude, double &longitude, double &altitude,
double &Velocity, double &FltEl, double &FltAz)
{
   double LE,ce,se;
   double EFG[3],VEFG[3],tmp[3];

   // Locate 0-deg Longitude relative to ECI-X
   LE = LE0 + wE*time*radToDeg;
   ce = cos(LE*degToRad);
   se = sin(LE*degToRad);

   // Convert to Earth Relative Velocity
   tmp[0] = VECI[0] + wE*ECI[1];
   tmp[1] = VECI[1] - wE*ECI[0];
   tmp[2] = VECI[2];

   // Coordinate transformation
   VEFG[0] =  ce*tmp[0]+se*tmp[1];
   VEFG[1] = -se*tmp[0]+ce*tmp[1];
   VEFG[2] = tmp[2];

   // Convert to EFG Coordinates
   EFG[0] =  ce*ECI[0]+se*ECI[1];
   EFG[1] = -se*ECI[0]+ce*ECI[1];
   EFG[2] = ECI[2];

   // Can now use library routine for earth relative position and vel.
   EFGv2llhv(EFG, VEFG, latitude, longitude, altitude, Velocity, FltEl, FltAz);

}
//-----------------------------------------------------------------------
// Function: EFGv2llhv
//
// Purpose: Convert from ECEF cartesian components of position and velocity
//          to local geodetic position and velocity
//
// Input:
//   EFG[3]               E, F, and G (m)
//   VEFG[3]              E, F, and G Velocity (m/s)
//
// Output:   (Passed by reference)
//   latitude             Geodetic latitude (deg)
//   longitude            Longitude (deg)
//   altitude             Height above ellipsoid (m)
//   Velocity             Magnitude (m/s)
//   FltAz                Flight Azimuth (deg)
//   FltEl                Flight Elevation (deg)
//-----------------------------------------------------------------------
void TGeoid::EFGv2llhv(double *EFG, double *VEFG,
                       double &latitude, double &longitude, double &altitude,
                       double &Velocity, double &FltEl, double &FltAz)
{
   double TegSite[9];
   double Vg[3],tmp;

   // Magnitude of velocity vector
   Velocity = norm2(VEFG);

   EFG2TxConstants(EFG,TegSite,latitude,longitude,altitude);

   // Transform velocity from ECEF components to local geodetic components
   matv(TegSite,VEFG,Vg,1);

   // Project velocity into local horizontal plane
   tmp = sqrt(Vg[0]*Vg[0]+Vg[1]*Vg[1]);

   // Can now use atan2 to compute flight elevation
   FltEl = safeAtan2(-Vg[2],tmp)*radToDeg;

   // Azimuth of velocity vector = azimuth of horizontal projection of velocity
   FltAz = safeAtan2(Vg[1],Vg[0])*radToDeg;
   if(FltAz < 0.0) FltAz += 360.0;
}
//-----------------------------------------------------------------------
// Function: llh2RAE
//
// Purpose: Slant Range/Look Azimuth/Look Elevation between two positions
//          from lat/lon/alt coordinates
//
// Input:
//   lat0,lon0      Reference Geodetic latitude, longitude (deg)
//   alt0           and altitude (m)
//   lat1,lon1      Target Geodetic latitude, longitude (deg)
//   alt1           and altitude (m)
//
// Output:   (Passed by reference)
//  Range	    Slant Range from Reference to Target (m)
//  Azimuth	    Look Azimuth from Reference to Target (d)
//  Elevation       Look Elevation from Reference to Target (d)
//-----------------------------------------------------------------------
void TGeoid::llh2RAE(double lat0, double lon0, double alt0,
           double lat1, double lon1, double alt1,
           double &Range, double &Azimuth, double &Elevation )
{
   int i;
   double Teg0[9],EFG0[3],EFG1[3];
   double dRe[3],dRg[3];
   double Rxy;

   // Compute Starting Point ECEF-EFG coodinates and local geodetic frame
   // direction cosines.
   llh2TxConstants(lat0,lon0,alt0,EFG0,Teg0);

   // Compute Finish Point ECEF-EFG coordinates
   llh2EFG(lat1,lon1,alt1,EFG1);

   // Compute Look Vector from Start(Pt0) to Finish (Pt1) (in ECEF coordinates)
   for(i = 0;i<3;i++) {
     dRe[i] = EFG1[i] - EFG0[i];
   }

   // Magnitude of Look Vector = Range
   Range = norm2(dRe);

   // Transform Look Vector into Local Geodetic components @ Pt0
   matv(Teg0,dRe,dRg,1);

   // Magnitude of look vector projection in local horizontal plane @ Pt0
   Rxy =  sqrt(dRg[0]*dRg[0]+dRg[1]*dRg[1]);

   // Can now use atan2 to compute elevation of look vector
   Elevation = safeAtan2(-dRg[2],Rxy)*radToDeg;

   // Azimuth of look vector = azimuth of horizontal projection of look vector
   Azimuth = safeAtan2(dRg[1],dRg[0])*radToDeg;
   if(Azimuth < 0.0) Azimuth += 360.0;
}
//-----------------------------------------------------------------------
// Function: RAE2llh
//
// Purpose: Compute target geodetic coordinates (lat,lon,alt) from a
//          reference point (lat, lon, alt) and a look vector (range,az,el)
//
// Input:
//   lat0,lon0      Reference Geodetic latitude, longitude (deg)
//   alt0           and altitude (m)
//   Range	    Slant Range from Reference to Target (m)
//   Azimuth	    Look Azimuth from Reference to Target (d)
//   Elevation       Look Elevation from Reference to Target (d)
//
// Output:   (Passed by reference)
//   lat1,lon1      Target Geodetic latitude, longitude (deg)
//   alt1           and altitude (m)
//-----------------------------------------------------------------------
void TGeoid::RAE2llh(double lat0, double lon0, double alt0,
             double Range, double Azimuth, double Elevation ,
             double &lat1, double &lon1, double &alt1)
{
   int i;
   double Teg0[9],EFG0[3],EFG1[3];
   double dRe[3],dRg[3];

   // Get Start ECEF Coordinates and Direction Cosines of North-East-Down
   llh2TxConstants(lat0,lon0,alt0,EFG0,Teg0);

   // Compute Range Vector in North-East-Down Coordinate System
   dRg[0] = Range*cos(Elevation*degToRad)*cos(Azimuth*degToRad);
   dRg[1] = Range*cos(Elevation*degToRad)*sin(Azimuth*degToRad);
   dRg[2] = -Range*sin(Elevation*degToRad);

   // Transform to ECEF Coordinate System
   matv(Teg0,dRg,dRe,0);

   // Add to Start Position Vector to get Final Position in ECEF Coordinates
   for(i=0;i<3;i++) {
      EFG1[i] = EFG0[i] + dRe[i];
   }

   // Transform to Geodetic Lat, Long and Alt
   EFG2llh(EFG1,lat1,lon1,alt1);

}

//-----------------------------------------------------------------------
// Function: llh2TxConstants
//
// Purpose:  Compute Geodetic Transformation Constants from
//           Tracking Site Latitude, Longitude and Altitude Coordinates
//
// Input:
//   latitude           Geodetic latitude (deg)
//   longitude          Longitude (deg)
//   altitude           Height above ellipsoid (m)
//
// Output:
//   EFGSite[3]         Site E, F, and G coordinates in metres
//   TegSite[9]         Site Direction Cosine matrix (row-wise) of
//                      North-East-Down Coordinate system wrt EFG
//
//-----------------------------------------------------------------------
void TGeoid::llh2TxConstants(double latitude, double longitude, double altitude,
		     double *EFGSite,double *TegSite)
{
   double sph,cph,slm,clm;

// EFG to Lat/Lon/Alt Conversion
   llh2EFG(latitude,longitude,altitude,EFGSite);

// Do Trig Once
   sph = sin(latitude*degToRad);
   cph = cos(latitude*degToRad);
   slm = sin(longitude*degToRad);
   clm = cos(longitude*degToRad);

// Geodetic (North-East-Down) to ECEF-EFG Transformation Matrix (Row Wise)
   TegSite[0] = -sph*clm;
   TegSite[1] = -slm;
   TegSite[2] = -cph*clm;
   TegSite[3] = -sph*slm;
   TegSite[4] = clm;
   TegSite[5] = -cph*slm;
   TegSite[6] = cph;
   TegSite[7] = 0.0;
   TegSite[8] = -sph;

}
//-----------------------------------------------------------------------
// Function: EFG2Teg
//
// Purpose:  Compute Geodetic Transformation Matrix from
//           Site EFG Coordinates
//
// Input:
//   EFGSite[3]    Site E, F, and G coordinates in meters
//
// Output:
//   TegSite[9]    Site Direction Cosine matrix of
//                 North-East-Down Coordinate system wrt EFG
//
//-----------------------------------------------------------------------
void TGeoid::EFG2Teg(double *EFGSite,double *TegSite)
{
   double latitude,longitude,altitude;
   EFG2TxConstants(EFGSite,TegSite,latitude,longitude,altitude);
}
//-----------------------------------------------------------------------
// Function: EFG2TxConstants
//
// Purpose:  Compute Geodetic Transformation Matrix and Geodetic Coordinates
//           from Site EFG Coordinates
//
// Input:
//   EFGSite[3]    Site E, F, and G coordinates in meters
//
// Output:
//   TegSite[9]    Site Direction Cosine matrix of
//                 North-East-Down Coordinate system wrt EFG
//   latitude, longitude, altitude Geodetic Coordinates
//
//-----------------------------------------------------------------------
void TGeoid::EFG2TxConstants(double *EFGSite,double *TegSite,
                     double &latitude, double &longitude, double &altitude)
{
   double sph,cph,slm,clm;

// EFG to Lat/Lon/Alt Conversion
   EFG2llh(EFGSite,latitude,longitude,altitude);

// Direction Cosines Stored Row-Wise
   sph = sin(latitude*degToRad);
   cph = cos(latitude*degToRad);
   slm = sin(longitude*degToRad);
   clm = cos(longitude*degToRad);

// Geodetic to EFG Transformation Matrix
   TegSite[0] = -sph*clm;
   TegSite[1] = -slm;
   TegSite[2] = -cph*clm;
   TegSite[3] = -sph*slm;
   TegSite[4] = clm;
   TegSite[5] = -cph*slm;
   TegSite[6] = cph;
   TegSite[7] = 0.0;
   TegSite[8] = -sph;
}
//-----------------------------------------------------------------------
// Function: RAE2EFG
//
// Purpose: Compute target ECEF cartesion position coordinates given
//          reference coordinates/transformation matrix and look vector.
//
// Input:
//   Range               Slant Range from measurement Site (m)
//   Azimuth             Look-Azimuth from Site to Target(deg)
//   Elevation           Look-Elevation from Site to Target(deg)
//   EFGSite[3]          Site EFG Coordinates (m)
//   TegSite[9]          Site Transformation Matrix
//
// Output:
//   EFG[3]              E, F, and G coordinates of target (m)
//
//-----------------------------------------------------------------------
void TGeoid::RAE2EFG(double Range, double Azimuth, double Elevation,
	     double *EFGSite, double *TegSite, double *EFG)
{
   int i,j;
   double ce,se,ca,sa,Xg[3];

   // Do all trig once.
   ce = cos(Elevation*degToRad);
   se = sin(Elevation*degToRad);
   ca = cos(Azimuth*degToRad);
   sa = sin(Azimuth*degToRad);

   // Geodetic - North-East-Down (NED) Coordinates
   Xg[0] = Range*ce*ca;
   Xg[1] = Range*ce*sa;
   Xg[2] = -Range*se;

   // Transform from Geodetic-NED to ECEF-EFG
   for(i=0; i<3; i++) {
      EFG[i] = EFGSite[i];
      for(j=0; j<3; j++) {
	 EFG[i] = EFG[i] + TegSite[3*i+j]*Xg[j];
      }
   }
}
//-----------------------------------------------------------------------
// Function: EFG2Kepler
//
// Purpose:  Compute Keplerian Orbital Parameters from ECEF State Vector
//
//   NOTE: This function works for ELLIPTICAL orbits ONLY. Function returns
//   error status if it detects parabolic or hyperbolic orbit.
//
//   NOTE: This function will return present latitude/longitude for impact if
//   it is in a non-intersecting earth orbit.
//
// Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics," Dover
//            Publications, 1971.
//
// Input Arguments:
//   GHA0         Greenwich Hour Angle at Epoch (h)
//   t0           Elapsed time since epoch (s)
//   EFG0[3]      Cartesian Components of ECEF position (m)
//   VEFG0[3]     Cartesian Components of ECEF velocity (m/s)
//   ImpactTol    Altitude Tolerance for Impact Point Iteration (m)
//
// Output Arguments (Passed By Reference):
//   a           Semi-Major Axis (km)
//   p           Semi-Latus Rectum (km)
//   Ecc         Orbit Eccentricity
//   inclination Orbit Inclination (d)
//   apogee      True Apogee Altitude over Geoid (km)
//   perigee     True Perigee Altitude over Geoid (km)
//   nu0         True Anomaly at t0 (d)
//   nuE         True Anomaly at Epoch (d)
//   LonNode     Longitude of Ascending Node (d)
//   ArgPeri     Argument of Perigee (d)
//   LatImpact   Impact Latitude (d)
//   LonImpact   Impact Longitude (d)
//   TimeToImpact Time from Current Time to Impact
//   TimeToApogee Time from Current Time to Next Apogee
//   OrbitFlag   = 0 if orbit intersects earth, = 1 otherwise
//
//-----------------------------------------------------------------------
TGeoid::EKeplerStatus TGeoid::EFG2Kepler(double GHA0, double t0,
               double *EFG0, double *VEFG0,
               double ImpactTol, double &a, double &p, double &ecc,
               double &inclination, double &apogee, double &perigee,
               double &nu0, double &nuE, double &LonNode, double &ArgPeri,
               double &LatImpact, double &LonImpact,
               double &TimeToImpact, double &TimeToApogee,
               int &OrbitFlag)
{

   double h[3],P[3],Q[3],n[3];
   double rdotV,VdotV,r;
   double Energy, AngMom;
   double cnu0, cE0, E0,Eepoch;
   double cEi,Ei,cnui,snui,nui;
   double ce,se;
   double tmp;
   double r0[3],V0[3];
   double AltImpact;
   double EFGImpact[3];
   double lat,lon;
   double EFG[3];
   double RImpact;
   double Period;
   int i;
   int IterCount;

   //  Default Return Parameters
   TimeToImpact = 0.0;
   EFGImpact[0] = EFG0[0];
   EFGImpact[1] = EFG0[1];
   EFGImpact[2] = EFG0[2];
   OrbitFlag    = 0;
   EFG2llh(EFGImpact,LatImpact,LonImpact,AltImpact);

   // Switch from m to km for better numeric conditioning
   for(i=0;i<3;i++) {
     r0[i] = EFG0[i]*.001;
     V0[i] = VEFG0[i]*.001;
   }

   // Transform from Earth Relative to Inertial Velocity
   V0[0] = V0[0] - wE*r0[1];
   V0[1] = V0[1] + wE*r0[0];

   // Preliminaries
   rdotV = r0[0]*V0[0] + r0[1]*V0[1] + r0[2]*V0[2];
   VdotV = V0[0]*V0[0] + V0[1]*V0[1] + V0[2]*V0[2];
   r = norm2(r0);

   // Protect against Floating Point Exceptions due to zero Radius
   if(r < kDenomEps) {
      return ksZeroRadius;
   }

//  Angular Momentum, perigee, and perp. vector

   // h = r x v
   h[0] = r0[1]*V0[2]-r0[2]*V0[1];
   h[1] = r0[2]*V0[0]-r0[0]*V0[2];
   h[2] = r0[0]*V0[1]-r0[1]*V0[0];
   AngMom = norm2(h);

   if(AngMom < kDenomEps) {
      return ksZeroAngMom;
   }

   // Perigee Vector
   tmp = VdotV-mu/r;
   P[0] = tmp*r0[0]-rdotV*V0[0];
   P[1] = tmp*r0[1]-rdotV*V0[1];
   P[2] = tmp*r0[2]-rdotV*V0[2];

   // Normalize Perigee Vector while protecting against Circular Orbit Case.
   tmp = norm2(P);
   if(fabs(tmp) > kDenomEps) {
      for(i = 0; i<3; i++) {
         P[i] = P[i]/tmp;
      }
   }
   else {
      //   Note: For Circular Orbit VdotV = mu/r and rdotV = 0.
      //         Therefore Perigee vector above degenerates to zero vector
      //         so we arbitrarily pick present position to be perigee direction
      P[0] = r0[0]/r;
      P[1] = r0[1]/r;
      P[2] = r0[2]/r;
   }

   // Q = hxP/|h|  - Unit Vector Perpendicular to Perigee and Ang. Momentum
   Q[0] = (h[1]*P[2]-h[2]*P[1])/AngMom;
   Q[1] = (h[2]*P[0]-h[0]*P[2])/AngMom;
   Q[2] = (h[0]*P[1]-h[1]*P[0])/AngMom;

   // Node vector = kxh
   tmp = sqrt(h[0]*h[0]+h[1]*h[1]);
   n[2] =  0.0;
   if(tmp < kDenomEps) {
      n[0] = 1.0;
      n[1] = 0.0;
   } else {
      n[0] = -h[1]/tmp;
      n[1] =  h[0]/tmp;
   }

   // Longitude of Ascending Node
   LonNode = sign(n[1])*safeAcos(n[0])*radToDeg;
   // Account for earth rotation
   LonNode = LonNode + wE*t0*radToDeg + GHA0*15.0;
   while(LonNode < 0.0) LonNode += 360.0;
   while(LonNode > 360.0) LonNode -= 360.0;


   // Argument of Perigee
   tmp = n[0]*P[0] + n[1]*P[1];
   ArgPeri = sign(P[2])*safeAcos(tmp)*radToDeg;
   if(ArgPeri < 0.0) ArgPeri += 360.0;

   // Orbital Energy  (km^2/s^2)
   Energy = VdotV/2.0-mu/r;
   if(Energy > 0.0) {
      return ksNonEllipticalOrbit;        // Non-elliptical orbit is not handled
   }


   // Orbit Semi-Major Axis radius (km)
   a = -mu/2.0/Energy;

   // Orbit semi-latus rectum (km)
   p = AngMom*AngMom/mu;

   // Compute Orbit eccentricity and protect against poor numerical conditioning
   // for very nearly circular orbits. 
   if((1.0-p/a) > 0.0 ) {
      ecc = sqrt(1.0-p/a);
   }
   else {
      ecc = 0.0;
   }

   // Orbit Inclination (rad)
   if(fabs(AngMom) > kDenomEps) {
      inclination = safeAcos(h[2]/AngMom);
   }
   else {
      inclination = M_PI/2.0*sign(h[2]);
   }

   // Locate Perigee and Compute Height Above Ellipsoid (km)
   for(i=0; i<3; i++) {
      EFG[i] = a*(1.0-ecc)*P[i]*1000.0;
   }
   EFG2llh(EFG,lat,lon,perigee);
   perigee *= 0.001;

   // Locate Apogee and compute height above ellipsoid (km)
   for(i=0; i<3; i++) {
      EFG[i] = a*(1.0+ecc)*P[i]*1000.0;
   }
   EFG2llh(EFG,lat,lon,apogee);
   apogee *= 0.001;

   // Set orbit flag based upon sign of perigee height above ellipsoid
   OrbitFlag = 0;
   if(perigee > 0.0) OrbitFlag = 1;
   Period = a*sqrt(a/mu);

   // True Anomally at Thrust Termination
   cnu0 = (P[0]*r0[0]+P[1]*r0[1]+P[2]*r0[2])/r;
   nu0 = safeAcos(cnu0);
   if(rdotV < 0.0) nu0 = 2.0*M_PI - nu0;

   // Eccentric Anomally at Thrust Termination
   cE0 = (ecc+cnu0)/(1.0+ecc*cnu0);
   E0 = safeAcos(cE0);
   if(rdotV < 0.0) E0 = -E0+2.0*M_PI;

    // Iterate for Impact Coordinates
   IterCount = 0;
   AltImpact = ImpactTol + 10.0;
   while(OrbitFlag == 0 && IterCount < kMaxIterate && AltImpact > ImpactTol) {
      // Compute ECEF Coordinates of Current Guess
      llh2EFG(LatImpact,LonImpact,0.0,EFGImpact);

      // Earth radius is used to determine where in the orbit impact occurs
      RImpact = 0.001*norm2(EFGImpact);

      // True Anomally at Impact - based upon earth radius seed
      if(ecc > kDenomEps) {
         cnui = (p/RImpact-1.0)/ecc;
       }
      else {
         cnui = 1.0;
      }
      nui = safeAcos(cnui);

      // Impact occurs on downleg of trajectory
      nui = -nui+2.0*M_PI;
      snui = sin(nui);

      // Eccentric Anomally at Impact - based upon earth radius seed
      if(ecc > kDenomEps) {
         cEi = (1.0-RImpact/a)/ecc;
      }
      else {
         cEi = 1.0;
      }
      Ei = safeAcos(cEi);

      // Adjust for downleg impact
      Ei = -Ei+2.0*M_PI;

      // Vacuum flight time to impact
      TimeToImpact = Period*(Ei-E0+ecc*(sin(E0)-sin(Ei)));

      // Inertial Impact Position (km)
      h[0] = RImpact*(cnui*P[0]+snui*Q[0]);
      h[1] = RImpact*(cnui*P[1]+snui*Q[1]);
      h[2] = RImpact*(cnui*P[2]+snui*Q[2]);

      //  Transform to Earth Relative Impact Position in meters
      ce = cos(wE*TimeToImpact);
      se = sin(wE*TimeToImpact);
      EFGImpact[0] = (h[0]*ce+h[1]*se)*1000.;
      EFGImpact[1] = (-h[0]*se+h[1]*ce)*1000.;
      EFGImpact[2] =  h[2]*1000.;

      // Compute Geodetic coordinates of impact. Will continue iteration if
      // altitude is not sufficiently close to zero.
      EFG2llh(EFGImpact,LatImpact,LonImpact,AltImpact);
      AltImpact = fabs(AltImpact);
      IterCount++;
   }

   //
   // Solve for Eccentric Anomaly at Elapsed Time = 0.0
   // Equation: f(Eepoch) = E-ecc*Sin(E) - (E0-ecc*sin(E0)-t0/Period) = 0
   //

   // Compute constant
   tmp = E0-ecc*sin(E0) - t0/Period;

   // Seed value for iteration
   Eepoch = tmp + ecc*sin(nu0-t0/Period);

   // Iterate 5 times using Newton's Method - df/dE analytically employed
   for(i=0;i<5;i++) {
         Eepoch = Eepoch - (Eepoch-ecc*sin(Eepoch)-tmp)/(1.0-ecc*cos(Eepoch));
   }

   // True Anomally at Epoch can now be computed
   cnu0 = (cos(Eepoch)-ecc)/(1.0-ecc*cos(Eepoch));
   nuE = safeAcos(cnu0)*radToDeg;
   if(sin(Eepoch) < 0.0) nuE = 360.0 - nuE;

   // Vacuum flight time to Apogee
   TimeToApogee = Period*(M_PI-E0+ecc*sin(E0));

   // Convert output angles from radians to degrees
   inclination = inclination*radToDeg;
   nu0 = nu0*radToDeg;

   return ksNormalExit;

}
//-----------------------------------------------------------------------
// Function: EFG2Kepler
//
// Purpose:  Compute Keplerian Orbital Parameters from ECEF State Vector
//
// Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics," Dover
//            Publications, 1971.
//
// Usage:    See Notes Above.
//
// NOTE: This Function is provided for backward compatibilty with old library
// calls. Newest EFG2Kepler function includes TimeToImpact and TimeToApogee
// as return arguments.
//-----------------------------------------------------------------------
TGeoid::EKeplerStatus TGeoid::EFG2Kepler(double GHA0, double t0,
               double *EFG0, double *VEFG0,
               double ImpactTol, double &a, double &p, double &ecc,
               double &inclination, double &apogee, double &perigee,
               double &nu0, double &nuE, double &LonNode, double &ArgPeri,
               double &LatImpact, double &LonImpact,
               int &OrbitFlag)
{
   double TimeToImpact,TimeToApogee;
   // Call Main Library Function, discarding time to impact & apogee
   return EFG2Kepler(GHA0, t0, EFG0, VEFG0, ImpactTol, a, p, ecc, inclination,
              apogee, perigee, nu0, nuE, LonNode, ArgPeri, LatImpact, LonImpact,
              TimeToImpact, TimeToApogee, OrbitFlag);
}
//-----------------------------------------------------------------------
// Function: Kepler2EFG
//
// Purpose:  Compute State Vector from Keplerian Orbital Elements
//
//   NOTE: This function works for ELLIPTICAL orbits ONLY.
//
// Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics," Dover
//            Publications, 1971.
//
// Input Arguments:
//   GHA0         Greenwich Hour Angle at Epoch (h)
//   t            Elapsed time since epoch (s)
//   a           Semi-Major Axis (km)
//   ecc         Orbit Eccentricity
//   nu0         True Anomaly at Epoch (d)
//   Inclination Orbit Inclination (d)
//   nu          True Anomaly at time = t (d)
//   LonNode     Longitude of Ascending Node (d)
//   ArgPeri     Argument of Perigee (d)
//
// Output Arguments:
//   EFG0[3]      EFG Coordinates (m)
//   VEFG0[3]     EFG Velocity (m/s)
//
//-----------------------------------------------------------------------
TGeoid::EKeplerStatus TGeoid::Kepler2EFG(double GHA0, double t, double a, double ecc, double nu0,
                double Inclination, double LonNode, double ArgPeri,
               double &nu, double *EFG, double *VEFG)
{
   int i;
   double p, E0, cE0, E, Period,cnu0,cnu,snu,tmp,sph0,cph0;
   double r,rp[3],ri[3],vp[3],vi[3];
   double Tip[9],Tei[9],co,cz,ci,so,sz,si;

   if(ecc < 0.0 || ecc >= 1.0 || a <= 0.0) {
      // If Elliptical Orbit Parameters are Out Of Bounds -
      // Do not modify EFG or VEFG and return Error Status.
      return ksNonEllipticalOrbit;
   }

   p = a*(1.0-ecc*ecc);
   cnu0 = cos(nu0*degToRad);
   // Compute eccentric anomally at epoch
   cE0 = (ecc+cnu0)/(1.0+ecc*cnu0);
   E0 = sign(sin(nu0*degToRad))*safeAcos(cE0);
   Period = a*sqrt(a/mu);

   // Eccentric anomally at elapsed time from epoch using Newton Iteration
   tmp = E0-ecc*sin(E0) + t/Period;
   E = tmp;
   for(i=0;i<5;i++) {
      E = E - (E-ecc*sin(E)-tmp)/(1.0-ecc*cos(E));
   }

   // True anomally at elapsed time from epoch
   cnu = (cos(E)-ecc)/(1.0-ecc*cos(E));
   nu = sign(sin(E))*safeAcos(cnu);
   snu = sin(nu);
   nu = nu*radToDeg;
   if(nu < 0.0) nu+= 360.0;

   r = p/(1.0+ecc*cnu);

   // R and V in perifocal coordinate system
   rp[0] = r*cnu;
   rp[1] = r*snu;
   rp[2] = 0.0;
   vp[0] = -sqrt(mu/p)*snu;
   vp[1] = sqrt(mu/p)*(ecc+cnu);
   vp[2] = 0.0;

   // Perifocal to inertial coordinate transformation
   co = cos(ArgPeri*degToRad);
   so = sin(ArgPeri*degToRad);

   cz = cos(LonNode*degToRad);
   sz = sin(LonNode*degToRad);

   ci = cos(Inclination*degToRad);
   si = sin(Inclination*degToRad);

   Tip[0] = cz*co-sz*so*ci;
   Tip[1] = -cz*so-sz*co*ci;
   Tip[2] = sz*si;
   Tip[3] = sz*co+cz*so*ci;
   Tip[4] = -sz*so+cz*co*ci;
   Tip[5] = -cz*si;
   Tip[6] = so*si;
   Tip[7] = co*si;
   Tip[8] = ci;

   // Inertial to ECEF transformation
   cph0 = cos(GHA0*15.0*degToRad + wE*t);
   sph0 = sin(GHA0*15.0*degToRad + wE*t);
   Tei[0] = cph0;
   Tei[1] = sph0;
   Tei[2] = 0.0;
   Tei[3] = -sph0;
   Tei[4] = cph0;
   Tei[5] = 0.0;
   Tei[6] = 0.0;
   Tei[7] = 0.0;
   Tei[8] = 1.0;

   // Transform from perifocal to inertial frame
   matv(Tip,rp,ri,0);
   matv(Tip,vp,vi,0);

   //Transform from Inertial to ECEF Components
   matv(Tei,ri,EFG,0);
   matv(Tei,vi,VEFG,0);

   // Convert from Inertial to Earth Relative Velocity
   VEFG[0] = VEFG[0] + wE*EFG[1];
   VEFG[1] = VEFG[1] - wE*EFG[0];

   // Convert from km to meters
   for(i=0;i<3;i++) {
      EFG[i] *= 1000.0;
      VEFG[i] *= 1000.0;
   }

   return ksNormalExit;
}
//-----------------------------------------------------------------------
//  Function: EFG2Gravity
//
//  Purpose: Compute Gravitational Acceleration at a Point
//
//  Input Argument:
//   x[3]     ECEF Cartesion Location (m)
//
//  Output Argument:
//   gE[3]  ECEF Cartesion Components of Gravitational Acceleration (m/s^2)
//-----------------------------------------------------------------------
void TGeoid::EFG2Gravity(double *x, double *gE) {
   double rI,spc,cpc,cle,sle,aRI,gr,gph,muR2,rxy;
   double aRI2,aRI3,aRI4;
   double spc2,spc3,spc4;

   // Length of Radius Vector
   rI  = norm2(x);

   // Protect against Floating Point Exception due to bad coordinate data
   if(rI < kDenomEps) {
      gE[0] = gE[1] = gE[2] = 0.0;
      return;
   }

   // Radius Vector projection on equatorial plane (m)
   rxy = sqrt(x[0]*x[0]+x[1]*x[1]);

   // Compute sin and cos of geocentric latitude
   spc = x[2]/rI;
   cpc = rxy/rI;

   // Compute sin and cos of geodetic longitude
   if(rxy >= kDenomEps) {
      sle = x[1]/rxy;
      cle = x[0]/rxy;
   }
   else {
      sle = 0.0;
      cle = 1.0;
   }

   // mu/R^2 (m/s^2)
   muR2 = mu/(0.001*rI)/(0.001*rI)*1000.;

   // Equatorial Radius to Position Radius Ratio
   aRI = a/rI;

   // Powers of a/rI
   aRI2 = aRI*aRI;
   aRI3 = aRI2*aRI;
   aRI4 = aRI3*aRI;

   // Powers of spc
   spc2 = spc*spc;
   spc3 = spc2*spc;
   spc4 = spc3*spc;

   // Radial Component of Gravitational Accel
   gr = muR2*(1.0 - 1.5*J2*aRI2*(3.0*spc2-1.0)
                  - 2.0*J3*aRI3*(5.0*spc2-3.0)*spc
                  -0.625*J4*aRI4*(35.0*spc4-30.0*spc2+3.0) );

   // Tangential Component of Gravitational Accel
   gph = muR2*(3.0*J2*aRI2*spc +
               0.5*J3*aRI3*(15.0*spc2-3.0) +
               2.5*J4*aRI4*(7.0*spc2-3.0)*spc)*cpc;

   // Transform into ECEF Components
   gE[0] = (gph*spc - gr*cpc)*cle ;
   gE[1] = (gph*spc - gr*cpc)*sle ;
   gE[2] = -gph*cpc - gr*spc     ;

}
//-----------------------------------------------------------------------
//  Function: EFGv2RotAcc
//
//  Purpose: Compute Coriolis and Centripetal Acceleration due to Earth Rotation
//
//  Input Arguments:
//   x[3]     ECEF Cartesion Location (m)
//   v[3]     ECEF Cartesion Velocity Components (m/s)
//
//  Output Argument:
//   arot[3]  ECEF Cartesion Components of Acceleration (m/s^2)
//-----------------------------------------------------------------------
void TGeoid::EFGv2RotAcc(double *x, double *v, double *arot) {
   double w[3],wxr[3],wxwxr[3];

   // ECEF Components of Earth Rotation Vector
   w[0] = 0.0;
   w[1] = 0.0;
   w[2] = wE;

   // Intermediate Vectors
   cross(w,v,arot);
   cross(w,x,wxr);
   cross(w,wxr,wxwxr);

   // Final Answer = 2* (wE x V) + wE x wE x r  (assumes wE is constant)
   for(int i=0;i<3;i++) {
      arot[i] = 2.0*arot[i]+wxwxr[i];
   }

}

//************************************************************************
// Utility Functions
//************************************************************************
//-----------------------------------------------------------------------
// Function: cross
//
// Purpose:  Vector Cross Product : c = a x b
//
//-----------------------------------------------------------------------
void TGeoid::cross(double *a, double *b,double *c)
{
   c[0] = a[1]*b[2] - a[2]*b[1];
   c[1] = a[2]*b[0] - a[0]*b[2];
   c[2] = a[0]*b[1] - a[1]*b[0];
}
//-----------------------------------------------------------------------
// Function: dot
//
// Purpose:  Vector Dot Product : c = a . b
//
//-----------------------------------------------------------------------
double TGeoid::dot(double *a, double *b)
{
   return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}
//-----------------------------------------------------------------------
// Function: matv
//
// Purpose:  Matrix Vector Multiplication
//
// Input:
//  a[9]       Row-wise 3x3 matrix
//  x[3]       Input Vector
//  itrn       Transpose Flag =0     b = [a].x
//                            =1     b = trn[a].x
//
// Output:
//  b          Ouput Vector
//-----------------------------------------------------------------------
void TGeoid::matv(double *a, double *x,double *b,const int itrn)
{
   for(int i=0; i<3; i++) {
      b[i] = 0.0;
      for(int j=0; j<3; j++) {
         if(itrn == 0) {
            b[i] = b[i] + a[3*i+j]*x[j];
         }
         else {
            b[i] = b[i] + a[3*j+i]*x[j];
         }
      }
   }
}
//-----------------------------------------------------------------------
// Function: norm2
//
// Purpose:  Compute Euclidean length of 3-space vector
//
//-----------------------------------------------------------------------
double TGeoid::norm2(double *a)
{
   return sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
}
//-----------------------------------------------------------------------
// Function: safeAtan2
//
// Purpose:  Compute ArcTan with Argument Checking
//
//-----------------------------------------------------------------------
double TGeoid::safeAtan2(double y, double x)
{
   if(x > kDenomEps || x < -kDenomEps) {
      return atan2(y,x);
   }
   else if( y >= 0.0) {
      return 0.5*M_PI;
   }

   return -0.5*M_PI;
}
//-----------------------------------------------------------------------
// Function: safeAcos
//
// Purpose:  Compute ArcCos with Argument Checking
//
//-----------------------------------------------------------------------
double TGeoid::safeAcos(double x)
{
   if(x <= 1.0 && x >= -1.0) {
      return acos(x);
   }
   else if(x > 1.0) {
      return 0.0;
   }
   return M_PI;
}


</nowiki>
=Internal Links=
<i>Parent Article:</i> [[Main Page]]<br />