YodaProfiler/src/sgp4.cpp

//
// globals.cpp
//
#include <sgp4.h>

//////////////////////////////////////////////////////////////////////////////
double sqr(const double x) 
{
   return (x * x);
}

//////////////////////////////////////////////////////////////////////////////
double Fmod2p(const double arg)
{
   double modu = fmod(arg, TWOPI);

   if (modu < 0.0)
      modu += TWOPI;

   return modu;
}

//////////////////////////////////////////////////////////////////////////////
// AcTan()
// ArcTangent of sin(x) / cos(x). The advantage of this function over arctan()
// is that it returns the correct quadrant of the angle.
double AcTan(const double sinx, const double cosx)
{
   double ret;

   if (cosx == 0.0)
   {
      if (sinx > 0.0)
         ret = PI / 2.0;
      else
         ret = 3.0 * PI / 2.0;
   }
   else
   {
      if (cosx > 0.0)
         ret = atan(sinx / cosx);
      else
         ret = PI + atan(sinx / cosx);
   }

   return ret;
}

//////////////////////////////////////////////////////////////////////////////
double rad2deg(const double r)
{
   const double DEG_PER_RAD = 180.0 / PI;
   return r * DEG_PER_RAD;
}

//////////////////////////////////////////////////////////////////////////////
double deg2rad(const double d)
{
   const double RAD_PER_DEG = PI / 180.0;
   return d * RAD_PER_DEG;
}

//
// coord.cpp
//
// Copyright (c) 2003 Michael F. Henry
//

//////////////////////////////////////////////////////////////////////
// cCoordGeo Class
//////////////////////////////////////////////////////////////////////

cCoordGeo::cCoordGeo()
{
   m_Lat = 0.0;
   m_Lon = 0.0;
   m_Alt = 0.0;
}

//////////////////////////////////////////////////////////////////////
// cCoordTopo Class
//////////////////////////////////////////////////////////////////////

cCoordTopo::cCoordTopo()
{
   m_Az = 0.0;
   m_El = 0.0;       
   m_Range = 0.0;    
   m_RangeRate = 0.0;

}


//
// cVector.cpp
//
// Copyright (c) 2001-2003 Michael F. Henry
//
//*****************************************************************************
// Multiply each component in the vector by 'factor'.
//*****************************************************************************
void cVector::Mul(double factor)
{
   m_x *= factor;
   m_y *= factor;
   m_z *= factor;
   m_w *= fabs(factor);
}

//*****************************************************************************
// Subtract a vector from this one.
//*****************************************************************************
void cVector::Sub(const cVector& vec)
{
   m_x -= vec.m_x;
   m_y -= vec.m_y;
   m_z -= vec.m_z;
   m_w -= vec.m_w;
}

//*****************************************************************************
// Calculate the angle between this vector and another
//*****************************************************************************
double cVector::Angle(const cVector& vec) const
{
  return acos(Dot(vec) / (Magnitude() * vec.Magnitude()));
}

//*****************************************************************************
//
//*****************************************************************************
double cVector::Magnitude() const
{
  return sqrt((m_x * m_x) + 
              (m_y * m_y) + 
              (m_z * m_z));
}

//*****************************************************************************
// Return the dot product
//*****************************************************************************
double cVector::Dot(const cVector& vec) const
{
   return (m_x * vec.m_x) +
          (m_y * vec.m_y) +
          (m_z * vec.m_z);
}
//
// cJulian.cpp
//
// This class encapsulates Julian dates with the epoch of 12:00 noon (12:00 UT)
// on January 1, 4713 B.C. Some epoch dates:
//    01/01/1990 00:00 UTC - 2447892.5
//    01/01/1990 12:00 UTC - 2447893.0
//    01/01/2000 00:00 UTC - 2451544.5
//    01/01/2001 00:00 UTC - 2451910.5
//
// Note the Julian day begins at noon, which allows astronomers to have all
// the dates in a single observing session the same.
//
// References:
// "Astronomical Formulae for Calculators", Jean Meeus
// "Satellite Communications", Dennis Roddy, 2nd Edition, 1995.
//
// Copyright (c) 2003 Michael F. Henry
//
// mfh 12/24/2003
//

//////////////////////////////////////////////////////////////////////////////
// Create a Julian date object from a time_t object. time_t objects store the
// number of seconds since midnight UTC January 1, 1970.
cJulian::cJulian(time_t time)
{
   struct tm *ptm = gmtime(&time);
   assert(ptm);

   int    year = ptm->tm_year + 1900;
   double day  = ptm->tm_yday + 1 +
                 (ptm->tm_hour + 
                  ((ptm->tm_min + 
                   (ptm->tm_sec / 60.0)) / 60.0)) / 24.0;

   Initialize(year, day);
}

//////////////////////////////////////////////////////////////////////////////
// Create a Julian date object from a year and day of year.
// Example parameters: year = 2001, day = 1.5 (Jan 1 12h)
cJulian::cJulian(int year, double day)
{
   Initialize(year, day);
}

//////////////////////////////////////////////////////////////////////////////
// Create a Julian date object.
cJulian::cJulian(int year,               // i.e., 2004
                 int mon,                // 1..12
                 int day,                // 1..31
                 int hour,               // 0..23
                 int min,                // 0..59
                 double sec /* = 0.0 */) // 0..(59.999999...)

{
   // Calculate N, the day of the year (1..366)
   int N;
   int F1 = (int)((275.0 * mon) / 9.0);
   int F2 = (int)((mon + 9.0) / 12.0);

   if (IsLeapYear(year))
   {
      // Leap year
      N = F1 - F2 + day - 30;
   }
   else
   {
      // Common year
      N = F1 - (2 * F2) + day - 30;
   }
   
   double dblDay = N + (hour + (min + (sec / 60.0)) / 60.0) / 24.0;

   Initialize(year, dblDay);
}

//////////////////////////////////////////////////////////////////////////////
void cJulian::Initialize(int year, double day)
{
   // 1582 A.D.: 10 days removed from calendar
   // 3000 A.D.: Arbitrary error checking limit
   assert((year > 1582) && (year < 3000));
   assert((day >= 0.0) && (day <= 366.5));

   // Now calculate Julian date

   year--;

   // Centuries are not leap years unless they divide by 400
   int A = (year / 100);
   int B = 2 - A + (A / 4);

   double NewYears = (int)(365.25 * year) +
                     (int)(30.6001 * 14)  + 
                     1720994.5 + B;  // 1720994.5 = Oct 30, year -1

   m_Date = NewYears + day;
}

//////////////////////////////////////////////////////////////////////////////
// getComponent()
// Return requested components of date.
// Year : Includes the century.
// Month: 1..12
// Day  : 1..31 including fractional part
void cJulian::getComponent(int    *pYear, 
                           int    *pMon  /* = NULL */,
                           double *pDOM  /* = NULL */) const
{
   assert(pYear != NULL);

   double jdAdj = getDate() + 0.5;
   int    Z     = (int)jdAdj;  // integer part
   double F     = jdAdj - Z;   // fractional part
   double alpha = (int)((Z - 1867216.25) / 36524.25);
   double A     = Z + 1 + alpha - (int)(alpha / 4.0);
   double B     = A + 1524.0;
   int    C     = (int)((B - 122.1) / 365.25);
   int    D     = (int)(C * 365.25);
   int    E     = (int)((B - D) / 30.6001);

   double DOM   = B - D - (int)(E * 30.6001) + F;
   int    month = (E < 13.5) ? (E - 1) : (E - 13);
   int    year  = (month > 2.5) ? (C - 4716) : (C - 4715); 
   
   *pYear = year;

   if (pMon != NULL)
      *pMon = month;

   if (pDOM != NULL)
      *pDOM = DOM;
} 

//////////////////////////////////////////////////////////////////////////////
// toGMST()
// Calculate Greenwich Mean Sidereal Time for the Julian date. The return value
// is the angle, in radians, measuring eastward from the Vernal Equinox to the
// prime meridian. This angle is also referred to as "ThetaG" (Theta GMST).
// 
// References:
//    The 1992 Astronomical Almanac, page B6.
//    Explanatory Supplement to the Astronomical Almanac, page 50.
//    Orbital Coordinate Systems, Part III, Dr. T.S. Kelso, Satellite Times,
//       Nov/Dec 1995
double cJulian::toGMST() const
{
   const double UT = fmod(m_Date + 0.5, 1.0);
   const double TU = (FromJan1_12h_2000() - UT) / 36525.0;

   double GMST = 24110.54841 + TU * 
                 (8640184.812866 + TU * (0.093104 - TU * 6.2e-06));

   GMST = fmod(GMST + SEC_PER_DAY * OMEGA_E * UT, SEC_PER_DAY);
   
   if (GMST < 0.0)
      GMST += SEC_PER_DAY;  // "wrap" negative modulo value

   return  (TWOPI * (GMST / SEC_PER_DAY));
 }

//////////////////////////////////////////////////////////////////////////////
// toLMST()
// Calculate Local Mean Sidereal Time for given longitude (for this date).
// The longitude is assumed to be in radians measured west from Greenwich.
// The return value is the angle, in radians, measuring eastward from the
// Vernal Equinox to the given longitude.
double cJulian::toLMST(double lon) const
{
   return fmod(toGMST() + lon, TWOPI);
}

//////////////////////////////////////////////////////////////////////////////
// toTime()
// Convert to type time_t
// Avoid using this function as it discards the fractional seconds of the
// time component.
time_t cJulian::toTime() const
{
   int    nYear;
   int    nMonth;
   double dblDay;

   getComponent(&nYear, &nMonth, &dblDay);

   // dblDay is the fractional Julian Day (i.e., 29.5577).
   // Save the whole number day in nDOM and convert dblDay to
   // the fractional portion of day.
   int nDOM = (int)dblDay;

   dblDay -= nDOM;

   const int SEC_PER_MIN = 60;
   const int SEC_PER_HR  = 60 * SEC_PER_MIN;
   const int SEC_PER_DAY = 24 * SEC_PER_HR;

   int secs = (int)((dblDay * SEC_PER_DAY) + 0.5);

   // Create a "struct tm" type. 
   // NOTE:
   // The "struct tm" type has a 1-second resolution. Any fractional
   // component of the "seconds" time value is discarded.
   struct tm tGMT;
   memset(&tGMT, 0, sizeof(tGMT));

   tGMT.tm_year = nYear - 1900;  // 2001 is 101
   tGMT.tm_mon  = nMonth - 1;    // January is 0
   tGMT.tm_mday = nDOM;          // First day is 1
   tGMT.tm_hour = secs / SEC_PER_HR;
   tGMT.tm_min  = (secs % SEC_PER_HR) / SEC_PER_MIN;
   tGMT.tm_sec  = (secs % SEC_PER_HR) % SEC_PER_MIN;
   tGMT.tm_isdst = 0; // No conversion desired

   time_t tEpoch = mktime(&tGMT);

   if (tEpoch != -1)
   {
      // Valid time_t value returned from mktime().
      // mktime() expects a local time which means that tEpoch now needs 
      // to be adjusted by the difference between this time zone and GMT.
      tEpoch -= timezone;
   }

   return tEpoch;
}
//
// cTle.cpp
// This class encapsulates a single set of standard NORAD two line elements.
//
// Copyright 1996-2005 Michael F. Henry
//
// Note: The column offsets are ZERO based.

// Name
const int TLE_LEN_LINE_DATA      = 69; const int TLE_LEN_LINE_NAME      = 22;

// Line 1
const int TLE1_COL_SATNUM        =  2; const int TLE1_LEN_SATNUM        =  5;
const int TLE1_COL_INTLDESC_A    =  9; const int TLE1_LEN_INTLDESC_A    =  2;
const int TLE1_COL_INTLDESC_B    = 11; const int TLE1_LEN_INTLDESC_B    =  3;
const int TLE1_COL_INTLDESC_C    = 14; const int TLE1_LEN_INTLDESC_C    =  3;
const int TLE1_COL_EPOCH_A       = 18; const int TLE1_LEN_EPOCH_A       =  2;
const int TLE1_COL_EPOCH_B       = 20; const int TLE1_LEN_EPOCH_B       = 12;
const int TLE1_COL_MEANMOTIONDT  = 33; const int TLE1_LEN_MEANMOTIONDT  = 10;
const int TLE1_COL_MEANMOTIONDT2 = 44; const int TLE1_LEN_MEANMOTIONDT2 =  8;
const int TLE1_COL_BSTAR         = 53; const int TLE1_LEN_BSTAR         =  8;
const int TLE1_COL_EPHEMTYPE     = 62; const int TLE1_LEN_EPHEMTYPE     =  1;
const int TLE1_COL_ELNUM         = 64; const int TLE1_LEN_ELNUM         =  4;

// Line 2
const int TLE2_COL_SATNUM        = 2;  const int TLE2_LEN_SATNUM        =  5;
const int TLE2_COL_INCLINATION   = 8;  const int TLE2_LEN_INCLINATION   =  8;
const int TLE2_COL_RAASCENDNODE  = 17; const int TLE2_LEN_RAASCENDNODE  =  8;
const int TLE2_COL_ECCENTRICITY  = 26; const int TLE2_LEN_ECCENTRICITY  =  7;
const int TLE2_COL_ARGPERIGEE    = 34; const int TLE2_LEN_ARGPERIGEE    =  8;
const int TLE2_COL_MEANANOMALY   = 43; const int TLE2_LEN_MEANANOMALY   =  8;
const int TLE2_COL_MEANMOTION    = 52; const int TLE2_LEN_MEANMOTION    = 11;
const int TLE2_COL_REVATEPOCH    = 63; const int TLE2_LEN_REVATEPOCH    =  5;

/////////////////////////////////////////////////////////////////////////////
cTle::cTle(string& strName, string& strLine1, string& strLine2)
{
   m_strName  = strName;
   m_strLine1 = strLine1;
   m_strLine2 = strLine2;

   Initialize();
}

/////////////////////////////////////////////////////////////////////////////
cTle::cTle(const cTle &tle)
{
   m_strName  = tle.m_strName;
   m_strLine1 = tle.m_strLine1;
   m_strLine2 = tle.m_strLine2;

   for (int fld = FLD_FIRST; fld < FLD_LAST; fld++)
   {
      m_Field[fld] = tle.m_Field[fld];
   }

   m_mapCache = tle.m_mapCache;
}

/////////////////////////////////////////////////////////////////////////////
cTle::~cTle()
{
}

/////////////////////////////////////////////////////////////////////////////
// getField()
// Return requested field as a double (function return value) or as a text
// string (*pstr) in the units requested (eUnit). Set 'bStrUnits' to true 
// to have units appended to text string.
//
// Note: numeric return values are cached; asking for the same field more
// than once incurs minimal overhead.
double cTle::getField(eField   fld, 
                      eUnits   units,    /* = U_NATIVE */
                      string  *pstr      /* = NULL     */,
                      bool     bStrUnits /* = false    */) const
{
   assert((FLD_FIRST <= fld) && (fld < FLD_LAST));
   assert((U_FIRST <= units) && (units < U_LAST));

   if (pstr)
   {
      // Return requested field in string form.
      *pstr = m_Field[fld];
      
      if (bStrUnits)
         *pstr += getUnits(fld);
      
      return 0.0;
   }
   else
   {
      // Return requested field in floating-point form.
      // Return cache contents if it exists, else populate cache
      FldKey key = Key(units, fld);

      if (m_mapCache.find(key) == m_mapCache.end())
      {
         // Value not in cache; add it
         double valNative = atof(m_Field[fld].c_str());
         double valConv   = ConvertUnits(valNative, fld, units); 
         m_mapCache[key]  = valConv;

         return valConv;
      }
      else
      {
         // return cached value
         return m_mapCache[key];
      }
   }
}

//////////////////////////////////////////////////////////////////////////////
// Convert the given field into the requested units. It is assumed that
// the value being converted is in the TLE format's "native" form.
double cTle::ConvertUnits(double valNative, // value to convert
                          eField fld,       // what field the value is
                          eUnits units)     // what units to convert to
{
   switch (fld)
   {
   case FLD_I:
   case FLD_RAAN:
   case FLD_ARGPER:
   case FLD_M:
     {
       // The native TLE format is DEGREES
       if (units == U_RAD)
         return valNative * RADS_PER_DEG;
     }
     
   case FLD_NORADNUM:
   case FLD_INTLDESC:
   case FLD_SET:
   case FLD_EPOCHYEAR:
   case FLD_EPOCHDAY:
   case FLD_ORBITNUM:
   case FLD_E:
   case FLD_MMOTION:
   case FLD_MMOTIONDT:
   case FLD_MMOTIONDT2:
   case FLD_BSTAR:
   case FLD_LAST:
     { // do nothing

     }

   }

   return valNative; // return value in unconverted native format
}

//////////////////////////////////////////////////////////////////////////////
string cTle::getUnits(eField fld) const 
{
   static const string strDegrees    = " degrees";
   static const string strRevsPerDay = " revs / day";
   static const string strNull;

   switch (fld)
   {
      case FLD_I:
      case FLD_RAAN:
      case FLD_ARGPER:
      case FLD_M:
         return strDegrees;

      case FLD_MMOTION:
         return strRevsPerDay;

      default:
         return strNull;   
   }
}

/////////////////////////////////////////////////////////////////////////////
// ExpToDecimal()
// Converts TLE-style exponential notation of the form [ |-]00000[+|-]0 to
// decimal notation. Assumes implied decimal point to the left of the first
// number in the string, i.e., 
//       " 12345-3" =  0.00012345
//       "-23429-5" = -0.0000023429   
//       " 40436+1" =  4.0436
string cTle::ExpToDecimal(const string &str)
{
   const int COL_EXP_SIGN = 6;
   const int LEN_EXP      = 2;
   
   const int LEN_BUFREAL  = 32;   // max length of buffer to hold floating point
                                 // representation of input string.
   int nMan;
   int nExp;      
   
   // sscanf(%d) will read up to the exponent sign
   sscanf(str.c_str(), "%d", &nMan);
   
   double dblMan = nMan;
   bool  bNeg    = (nMan < 0);
   
   if (bNeg)
      dblMan *= -1;
   
   // Move decimal place to left of first digit
   while (dblMan >= 1.0)
      dblMan /= 10.0;
   
   if (bNeg)
      dblMan *= -1;
   
   // now read exponent
   sscanf(str.substr(COL_EXP_SIGN, LEN_EXP).c_str(), "%d", &nExp);
   
   double dblVal = dblMan * pow(10.0, nExp);
   char szVal[LEN_BUFREAL];
   
   snprintf(szVal, sizeof(szVal), "%.9f", dblVal);
   
   string strVal = szVal;
   
   return strVal;
   
} // ExpToDecimal()

/////////////////////////////////////////////////////////////////////////////
// Initialize()
// Initialize the string array.
void cTle::Initialize()
{
   // Have we already been initialized?
   if (m_Field[FLD_NORADNUM].size())
      return;
   
   assert(!m_strName.empty());
   assert(!m_strLine1.empty());
   assert(!m_strLine2.empty());
   
   m_Field[FLD_NORADNUM] = m_strLine1.substr(TLE1_COL_SATNUM, TLE1_LEN_SATNUM);
   m_Field[FLD_INTLDESC] = m_strLine1.substr(TLE1_COL_INTLDESC_A,
                                             TLE1_LEN_INTLDESC_A +
                                             TLE1_LEN_INTLDESC_B +   
                                             TLE1_LEN_INTLDESC_C);   
   m_Field[FLD_EPOCHYEAR] = 
         m_strLine1.substr(TLE1_COL_EPOCH_A, TLE1_LEN_EPOCH_A);

   m_Field[FLD_EPOCHDAY] = 
         m_strLine1.substr(TLE1_COL_EPOCH_B, TLE1_LEN_EPOCH_B);
   
   if (m_strLine1[TLE1_COL_MEANMOTIONDT] == '-')
   {
      // value is negative
      m_Field[FLD_MMOTIONDT] = "-0";
   }
   else
      m_Field[FLD_MMOTIONDT] = "0";
   
   m_Field[FLD_MMOTIONDT] += m_strLine1.substr(TLE1_COL_MEANMOTIONDT + 1,
                                               TLE1_LEN_MEANMOTIONDT);
   
   // decimal point assumed; exponential notation
   m_Field[FLD_MMOTIONDT2] = ExpToDecimal(
                                 m_strLine1.substr(TLE1_COL_MEANMOTIONDT2,
                                                   TLE1_LEN_MEANMOTIONDT2));
   // decimal point assumed; exponential notation
   m_Field[FLD_BSTAR] = ExpToDecimal(m_strLine1.substr(TLE1_COL_BSTAR,
                                                       TLE1_LEN_BSTAR));
   //TLE1_COL_EPHEMTYPE      
   //TLE1_LEN_EPHEMTYPE   
   m_Field[FLD_SET] = m_strLine1.substr(TLE1_COL_ELNUM, TLE1_LEN_ELNUM);
   
   TrimLeft(m_Field[FLD_SET]);
   
   //TLE2_COL_SATNUM         
   //TLE2_LEN_SATNUM         
   
   m_Field[FLD_I] = m_strLine2.substr(TLE2_COL_INCLINATION,
                                      TLE2_LEN_INCLINATION);
   TrimLeft(m_Field[FLD_I]);
   
   m_Field[FLD_RAAN] = m_strLine2.substr(TLE2_COL_RAASCENDNODE,
                                         TLE2_LEN_RAASCENDNODE);
   TrimLeft(m_Field[FLD_RAAN]);

   // decimal point is assumed
   m_Field[FLD_E]   = "0.";
   m_Field[FLD_E]   += m_strLine2.substr(TLE2_COL_ECCENTRICITY,
                                       TLE2_LEN_ECCENTRICITY);
   
   m_Field[FLD_ARGPER] = m_strLine2.substr(TLE2_COL_ARGPERIGEE,
                                           TLE2_LEN_ARGPERIGEE);
   TrimLeft(m_Field[FLD_ARGPER]);
   
   m_Field[FLD_M]   = m_strLine2.substr(TLE2_COL_MEANANOMALY,
                                      TLE2_LEN_MEANANOMALY);
   TrimLeft(m_Field[FLD_M]);
   
   m_Field[FLD_MMOTION]   = m_strLine2.substr(TLE2_COL_MEANMOTION, 
                                            TLE2_LEN_MEANMOTION);
   TrimLeft(m_Field[FLD_MMOTION]);
   
   m_Field[FLD_ORBITNUM] = m_strLine2.substr(TLE2_COL_REVATEPOCH,
                                             TLE2_LEN_REVATEPOCH);
   TrimLeft(m_Field[FLD_ORBITNUM]);
   
} // InitStrVars()

/////////////////////////////////////////////////////////////////////////////
// IsTleFormat()
// Returns true if "str" is a valid data line of a two-line element set,
//   else false.
//
// To be valid a line must:
//      Have as the first character the line number
//      Have as the second character a blank
//      Be TLE_LEN_LINE_DATA characters long
//      Have a valid checksum (note: no longer required as of 12/96)
//      
bool cTle::IsValidLine(string& str, eTleLine line)
{
   TrimLeft(str);
   TrimRight(str);
   
   size_t nLen = str.size();
   
   if (nLen != (uint)TLE_LEN_LINE_DATA)
      return false;
   
   // First char in string must be line number
   if ((str[0] - '0') != line)
      return false;
   
   // Second char in string must be blank
   if (str[1] != ' ')
      return false;
   
   /*
      NOTE: 12/96 
      The requirement that the last char in the line data must be a valid 
      checksum is too restrictive. 
      
      // Last char in string must be checksum
      int nSum = CheckSum(str);
     
      if (nSum != (str[TLE_LEN_LINE_DATA - 1] - '0'))
         return false;
   */
   
   return true;
   
} // IsTleFormat()

/////////////////////////////////////////////////////////////////////////////
// CheckSum()
// Calculate the check sum for a given line of TLE data, the last character
// of which is the current checksum. (Although there is no check here,
// the current checksum should match the one we calculate.)
// The checksum algorithm: 
//      Each number in the data line is summed, modulo 10.
//    Non-numeric characters are zero, except minus signs, which are 1.
//
int cTle::CheckSum(const string& str)
{
   // The length is "- 1" because we don't include the current (existing)
   // checksum character in the checksum calculation.
   size_t len = str.size() - 1;
   int xsum = 0;
   
   for (size_t i = 0; i < len; i++)
   {
      char ch = str[i];
      if (isdigit(ch))
         xsum += (ch - '0');
      else if (ch == '-')
         xsum++;
   }
   
   return (xsum % 10);
   
} // CheckSum()

/////////////////////////////////////////////////////////////////////////////
void cTle::TrimLeft(string& s)
{
   while (s[0] == ' ')
      s.erase(0, 1);
}

/////////////////////////////////////////////////////////////////////////////
void cTle::TrimRight(string& s)
{
   while (s[s.size() - 1] == ' ')
      s.erase(s.size() - 1);
}

//
// cEci.cpp
//
// Copyright (c) 2002-2003 Michael F. Henry
//
//////////////////////////////////////////////////////////////////////
// cEci Class
//////////////////////////////////////////////////////////////////////
cEci::cEci(const cVector &pos, 
           const cVector &vel, 
           const cJulian &date,
           bool  IsAeUnits /* = true */)
{
   m_pos      = pos;
   m_vel      = vel;
   m_date     = date;
   m_VecUnits = (IsAeUnits ? UNITS_AE : UNITS_NONE);
}

//////////////////////////////////////////////////////////////////////
// cEci(cCoordGeo&, cJulian&)
// Calculate the ECI coordinates of the location "geo" at time "date".
// Assumes geo coordinates are km-based.
// Assumes the earth is an oblate spheroid as defined in WGS '72.
// Reference: The 1992 Astronomical Almanac, page K11
// Reference: www.celestrak.com (Dr. TS Kelso)
cEci::cEci(const cCoordGeo &geo, const cJulian &date)
{
   m_VecUnits = UNITS_KM;

   double mfactor = TWOPI * (OMEGA_E / SEC_PER_DAY);
   double lat = geo.m_Lat;
   double lon = geo.m_Lon;
   double alt = geo.m_Alt;

   // Calculate Local Mean Sidereal Time (theta)
   double theta = date.toLMST(lon);
   double c = 1.0 / sqrt(1.0 + F * (F - 2.0) * sqr(sin(lat)));
   double s = sqr(1.0 - F) * c;
   double achcp = (XKMPER_WGS72 * c + alt) * cos(lat);

   m_date = date;

   m_pos.m_x = achcp * cos(theta);               // km
   m_pos.m_y = achcp * sin(theta);               // km
   m_pos.m_z = (XKMPER_WGS72 * s + alt) * sin(lat);   // km
   m_pos.m_w = sqrt(sqr(m_pos.m_x) + 
                    sqr(m_pos.m_y) + 
                    sqr(m_pos.m_z));            // range, km

   m_vel.m_x = -mfactor * m_pos.m_y;            // km / sec
   m_vel.m_y =  mfactor * m_pos.m_x;
   m_vel.m_z = 0.0;
   m_vel.m_w = sqrt(sqr(m_vel.m_x) +            // range rate km/sec^2
                    sqr(m_vel.m_y));
}

//////////////////////////////////////////////////////////////////////////////
// toGeo()
// Return the corresponding geodetic position (based on the current ECI
// coordinates/Julian date).
// Assumes the earth is an oblate spheroid as defined in WGS '72.
// Side effects: Converts the position and velocity vectors to km-based units.
// Reference: The 1992 Astronomical Almanac, page K12. 
// Reference: www.celestrak.com (Dr. TS Kelso)
cCoordGeo cEci::toGeo()
{
   ae2km(); // Vectors must be in kilometer-based units

   double theta = AcTan(m_pos.m_y, m_pos.m_x);
   double lon   = fmod(theta - m_date.toGMST(), TWOPI);
   
   if (lon < 0.0) 
      lon += TWOPI;  // "wrap" negative modulo

   double r   = sqrt(sqr(m_pos.m_x) + sqr(m_pos.m_y));
   double e2  = F * (2.0 - F);
   double lat = AcTan(m_pos.m_z, r);

   const double delta = 1.0e-07;
   double phi;
   double c;

   do   
   {
      phi = lat;
      c   = 1.0 / sqrt(1.0 - e2 * sqr(sin(phi)));
      lat = AcTan(m_pos.m_z + XKMPER_WGS72 * c * e2 * sin(phi), r);
   }
   while (fabs(lat - phi) > delta);
   
   double alt = r / cos(lat) - XKMPER_WGS72 * c;

   return cCoordGeo(lat, lon, alt); // radians, radians, kilometers
}

//////////////////////////////////////////////////////////////////////////////
// ae2km()
// Convert the position and velocity vector units from AE-based units
// to kilometer based units.
void cEci::ae2km()
{
   if (UnitsAreAe())
   {
      MulPos(XKMPER_WGS72 / AE);                       // km
      MulVel((XKMPER_WGS72 / AE) * (MIN_PER_DAY / 86400));  // km/sec
      m_VecUnits = UNITS_KM;
   }
}