diff --git a/Eci.cpp b/Eci.cpp index f45c455..c17f556 100644 --- a/Eci.cpp +++ b/Eci.cpp @@ -1,82 +1,82 @@ -#include "Eci.h" - -#include "Util.h" - -void Eci::ToEci(const Julian& date, const CoordGeodetic &g) -{ - /* - * set date - */ - date_ = date; - - static const double mfactor = kTWOPI * (kOMEGA_E / kSECONDS_PER_DAY); - - /* - * Calculate Local Mean Sidereal Time for observers longitude - */ - const double theta = date_.ToLocalMeanSiderealTime(g.longitude); - - /* - * take into account earth flattening - */ - const double c = 1.0 - / sqrt(1.0 + kF * (kF - 2.0) * pow(sin(g.latitude), 2.0)); - const double s = pow(1.0 - kF, 2.0) * c; - const double achcp = (kXKMPER * c + g.altitude) * cos(g.latitude); - - /* - * X position in km - * Y position in km - * Z position in km - * W magnitude in km - */ - position_.x = achcp * cos(theta); - position_.y = achcp * sin(theta); - position_.z = (kXKMPER * s + g.altitude) * sin(g.latitude); - position_.w = position_.GetMagnitude(); - - /* - * X velocity in km/s - * Y velocity in km/s - * Z velocity in km/s - * W magnitude in km/s - */ - velocity_.x = -mfactor * position_.y; - velocity_.y = mfactor * position_.x; - velocity_.z = 0.0; - velocity_.w = velocity_.GetMagnitude(); -} - -CoordGeodetic Eci::ToGeodetic() const -{ - const double theta = Util::AcTan(position_.y, position_.x); - - // 0 >= lon < 360 - // const double lon = Fmod2p(theta - date_.ToGreenwichSiderealTime()); - // 180 >= lon < 180 - const double lon = Util::WrapNegPosPI(theta - date_.ToGreenwichSiderealTime()); - - const double r = sqrt((position_.x * position_.x) - + (position_.y * position_.y)); - - static const double e2 = kF * (2.0 - kF); - - double lat = Util::AcTan(position_.z, r); - double phi = 0.0; - double c = 0.0; - int cnt = 0; - - do - { - phi = lat; - const double sinphi = sin(phi); - c = 1.0 / sqrt(1.0 - e2 * sinphi * sinphi); - lat = Util::AcTan(position_.z + kXKMPER * c * e2 * sinphi, r); - cnt++; - } - while (fabs(lat - phi) >= 1e-10 && cnt < 10); - - const double alt = r / cos(lat) - kXKMPER * c; - - return CoordGeodetic(lat, lon, alt, true); -} +#include "Eci.h" + +#include "Util.h" + +void Eci::ToEci(const Julian& date, const CoordGeodetic &g) +{ + /* + * set date + */ + date_ = date; + + static const double mfactor = kTWOPI * (kOMEGA_E / kSECONDS_PER_DAY); + + /* + * Calculate Local Mean Sidereal Time for observers longitude + */ + const double theta = date_.ToLocalMeanSiderealTime(g.longitude); + + /* + * take into account earth flattening + */ + const double c = 1.0 + / sqrt(1.0 + kF * (kF - 2.0) * pow(sin(g.latitude), 2.0)); + const double s = pow(1.0 - kF, 2.0) * c; + const double achcp = (kXKMPER * c + g.altitude) * cos(g.latitude); + + /* + * X position in km + * Y position in km + * Z position in km + * W magnitude in km + */ + position_.x = achcp * cos(theta); + position_.y = achcp * sin(theta); + position_.z = (kXKMPER * s + g.altitude) * sin(g.latitude); + position_.w = position_.GetMagnitude(); + + /* + * X velocity in km/s + * Y velocity in km/s + * Z velocity in km/s + * W magnitude in km/s + */ + velocity_.x = -mfactor * position_.y; + velocity_.y = mfactor * position_.x; + velocity_.z = 0.0; + velocity_.w = velocity_.GetMagnitude(); +} + +CoordGeodetic Eci::ToGeodetic() const +{ + const double theta = Util::AcTan(position_.y, position_.x); + + // 0 >= lon < 360 + // const double lon = Fmod2p(theta - date_.ToGreenwichSiderealTime()); + // 180 >= lon < 180 + const double lon = Util::WrapNegPosPI(theta - date_.ToGreenwichSiderealTime()); + + const double r = sqrt((position_.x * position_.x) + + (position_.y * position_.y)); + + static const double e2 = kF * (2.0 - kF); + + double lat = Util::AcTan(position_.z, r); + double phi = 0.0; + double c = 0.0; + int cnt = 0; + + do + { + phi = lat; + const double sinphi = sin(phi); + c = 1.0 / sqrt(1.0 - e2 * sinphi * sinphi); + lat = Util::AcTan(position_.z + kXKMPER * c * e2 * sinphi, r); + cnt++; + } + while (fabs(lat - phi) >= 1e-10 && cnt < 10); + + const double alt = r / cos(lat) - kXKMPER * c; + + return CoordGeodetic(lat, lon, alt, true); +} diff --git a/Julian.cpp b/Julian.cpp index 5b204e7..f1177aa 100644 --- a/Julian.cpp +++ b/Julian.cpp @@ -1,306 +1,306 @@ -#include "Globals.h" -#include "Julian.h" -#include "Util.h" - -#include -#include -#include - -#ifdef WIN32 -#include -#else -#include -#endif - -Julian::Julian() -{ -#ifdef WIN32 - SYSTEMTIME st; - GetSystemTime(&st); - Initialize(st.wYear, - st.wMonth, - st.wDay, - st.wHour, - st.wMinute, - (double) st.wSecond + (double) st.wMilliseconds / 1000.0); -#else - struct timeval tv; - gettimeofday(&tv, NULL); - struct tm gmt; - gmtime_r(&tv.tv_sec, &gmt); - Initialize(gmt.tm_year + 1900, - gmt.tm_mon + 1, - gmt.tm_mday, - gmt.tm_hour, - gmt.tm_min, - (double) gmt.tm_sec + (double) tv.tv_usec / 1000000.0); -#endif -} - -/* - * create julian date given time_t value - */ -Julian::Julian(const time_t t) -{ - struct tm ptm; -#if WIN32 - if (gmtime_s(&ptm, &t)) - { - assert(1); - } -#else - if (gmtime_r(&t, &ptm) == NULL) - { - assert(1); - } -#endif - 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); -} - -/* - * comparison - */ -bool Julian::operator==(const Julian &date) const -{ - return date_ == date.date_ ? true : false; -} - -bool Julian::operator!=(const Julian &date) const -{ - return !(*this == date); -} - -bool Julian::operator>(const Julian &date) const -{ - return date_ > date.date_ ? true : false; -} - -bool Julian::operator<(const Julian &date) const { - - return date_ < date.date_ ? true : false; -} - -bool Julian::operator>=(const Julian &date) const -{ - return date_ >= date.date_ ? true : false; -} - -bool Julian::operator<=(const Julian &date) const -{ - return date_ <= date.date_ ? true : false; -} - -/* - * assignment - */ -Julian& Julian::operator=(const Julian& b) -{ - if (this != &b) { - date_ = b.date_; - } - return (*this); -} - -Julian& Julian::operator=(const double b) -{ - date_ = b; - return (*this); -} - -/* - * arithmetic - */ -Julian Julian::operator +(const Timespan& b) const -{ - return Julian(*this) += b; -} - -Julian Julian::operator-(const Timespan& b) const -{ - return Julian(*this) -= b; -} - -Timespan Julian::operator-(const Julian& b) const -{ - return Timespan(date_ - b.date_); -} - -/* - * compound assignment - */ -Julian & Julian::operator +=(const Timespan& b) -{ - date_ += b; - return (*this); -} - -Julian & Julian::operator -=(const Timespan& b) -{ - date_ -= b; - return (*this); -} - -/* - * create julian date from year and day of year - */ -void Julian::Initialize(int year, double day) -{ - year--; - - int A = (year / 100); - int B = 2 - A + (A / 4); - - double new_years = static_cast (365.25 * year) + - static_cast (30.6001 * 14) + - 1720994.5 + B; - - date_ = new_years + day; -} - -/* - * create julian date from individual components - * year: 2004 - * mon: 1-12 - * day: 1-31 - * hour: 0-23 - * min: 0-59 - * sec: 0-59.99 - */ -void Julian::Initialize(int year, int mon, int day, - int hour, int min, double sec) -{ - // 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); -} - -/* - * converts time to time_t - * note: resolution to seconds only - */ -time_t Julian::ToTime() const -{ - return static_cast ((date_ - 2440587.5) * 86400.0); -} - -/* - * Greenwich Mean Sidereal Time - */ -double Julian::ToGreenwichSiderealTime() const -{ -#if 0 - const double UT = fmod(jul + 0.5, 1.0); - const double TU = (jul - 2451545.0 - 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)); -#endif - - // tut1 = Julian centuries from 2000 Jan. 1 12h UT1 - // (since J2000 which is 2451545.0) - // a Julian century is 36525 days - const double tut1 = (date_ - 2451545.0) / 36525.0; - - // Rotation angle in arcseconds - double theta = 67310.54841 + (876600.0 * 3600.0 + 8640184.812866) * tut1 - + 0.093104 * pow(tut1, 2.0) - 0.0000062 * pow(tut1, 3.0); - - // 360.0 / 86400.0 = 1.0 / 240.0 - theta = Util::WrapTwoPI(Util::DegreesToRadians(theta / 240.0)); - - return theta; - -#if 0 - static const double C1 = 1.72027916940703639e-2; - static const double THGR70 = 1.7321343856509374; - static const double FK5R = 5.07551419432269442e-15; - - /* - * get integer number of days from 0 jan 1970 - */ - const double ts70 = date_ - 2433281.5 - 7305.0; - const double ds70 = floor(ts70 + 1.0e-8); - const double tfrac = ts70 - ds70; - /* - * find greenwich location at epoch - */ - const double c1p2p = C1 + kTWOPI; - double gsto = Util::WrapTwoPI(THGR70 + C1 * ds70 + c1p2p * tfrac - + ts70 * ts70 * FK5R); - - return gsto; -#endif -} - -/* - * Local Mean Sideral Time - */ -double Julian::ToLocalMeanSiderealTime(const double& lon) const -{ - return fmod(ToGreenwichSiderealTime() + lon, kTWOPI); -} - -void Julian::ToGregorian(struct DateTimeComponents* datetime) const -{ - double jdAdj = GetDate() + 0.5; - int Z = (int) jdAdj; - double F = jdAdj - Z; - - int A = 0; - - if (Z < 2299161) - { - A = static_cast (Z); - } - else - { - int a = static_cast ((Z - 1867216.25) / 36524.25); - A = static_cast (Z + 1 + a - static_cast (a / 4)); - } - - int B = A + 1524; - int C = static_cast ((B - 122.1) / 365.25); - int D = static_cast (365.25 * C); - int E = static_cast ((B - D) / 30.6001); - - datetime->hours = static_cast (F * 24.0); - F -= datetime->hours / 24.0; - datetime->minutes = static_cast (F * 1440.0); - F -= datetime->minutes / 1440.0; - datetime->seconds = F * 86400.0; - - datetime->days = B - D - static_cast (30.6001 * E); - datetime->months = E < 14 ? E - 1 : E - 13; - datetime->years = datetime->months > 2 ? C - 4716 : C - 4715; -} +#include "Globals.h" +#include "Julian.h" +#include "Util.h" + +#include +#include +#include + +#ifdef WIN32 +#include +#else +#include +#endif + +Julian::Julian() +{ +#ifdef WIN32 + SYSTEMTIME st; + GetSystemTime(&st); + Initialize(st.wYear, + st.wMonth, + st.wDay, + st.wHour, + st.wMinute, + (double) st.wSecond + (double) st.wMilliseconds / 1000.0); +#else + struct timeval tv; + gettimeofday(&tv, NULL); + struct tm gmt; + gmtime_r(&tv.tv_sec, &gmt); + Initialize(gmt.tm_year + 1900, + gmt.tm_mon + 1, + gmt.tm_mday, + gmt.tm_hour, + gmt.tm_min, + (double) gmt.tm_sec + (double) tv.tv_usec / 1000000.0); +#endif +} + +/* + * create julian date given time_t value + */ +Julian::Julian(const time_t t) +{ + struct tm ptm; +#if WIN32 + if (gmtime_s(&ptm, &t)) + { + assert(1); + } +#else + if (gmtime_r(&t, &ptm) == NULL) + { + assert(1); + } +#endif + 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); +} + +/* + * comparison + */ +bool Julian::operator==(const Julian &date) const +{ + return date_ == date.date_ ? true : false; +} + +bool Julian::operator!=(const Julian &date) const +{ + return !(*this == date); +} + +bool Julian::operator>(const Julian &date) const +{ + return date_ > date.date_ ? true : false; +} + +bool Julian::operator<(const Julian &date) const { + + return date_ < date.date_ ? true : false; +} + +bool Julian::operator>=(const Julian &date) const +{ + return date_ >= date.date_ ? true : false; +} + +bool Julian::operator<=(const Julian &date) const +{ + return date_ <= date.date_ ? true : false; +} + +/* + * assignment + */ +Julian& Julian::operator=(const Julian& b) +{ + if (this != &b) { + date_ = b.date_; + } + return (*this); +} + +Julian& Julian::operator=(const double b) +{ + date_ = b; + return (*this); +} + +/* + * arithmetic + */ +Julian Julian::operator +(const Timespan& b) const +{ + return Julian(*this) += b; +} + +Julian Julian::operator-(const Timespan& b) const +{ + return Julian(*this) -= b; +} + +Timespan Julian::operator-(const Julian& b) const +{ + return Timespan(date_ - b.date_); +} + +/* + * compound assignment + */ +Julian & Julian::operator +=(const Timespan& b) +{ + date_ += b; + return (*this); +} + +Julian & Julian::operator -=(const Timespan& b) +{ + date_ -= b; + return (*this); +} + +/* + * create julian date from year and day of year + */ +void Julian::Initialize(int year, double day) +{ + year--; + + int A = (year / 100); + int B = 2 - A + (A / 4); + + double new_years = static_cast (365.25 * year) + + static_cast (30.6001 * 14) + + 1720994.5 + B; + + date_ = new_years + day; +} + +/* + * create julian date from individual components + * year: 2004 + * mon: 1-12 + * day: 1-31 + * hour: 0-23 + * min: 0-59 + * sec: 0-59.99 + */ +void Julian::Initialize(int year, int mon, int day, + int hour, int min, double sec) +{ + // 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); +} + +/* + * converts time to time_t + * note: resolution to seconds only + */ +time_t Julian::ToTime() const +{ + return static_cast ((date_ - 2440587.5) * 86400.0); +} + +/* + * Greenwich Mean Sidereal Time + */ +double Julian::ToGreenwichSiderealTime() const +{ +#if 0 + const double UT = fmod(jul + 0.5, 1.0); + const double TU = (jul - 2451545.0 - 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)); +#endif + + // tut1 = Julian centuries from 2000 Jan. 1 12h UT1 + // (since J2000 which is 2451545.0) + // a Julian century is 36525 days + const double tut1 = (date_ - 2451545.0) / 36525.0; + + // Rotation angle in arcseconds + double theta = 67310.54841 + (876600.0 * 3600.0 + 8640184.812866) * tut1 + + 0.093104 * pow(tut1, 2.0) - 0.0000062 * pow(tut1, 3.0); + + // 360.0 / 86400.0 = 1.0 / 240.0 + theta = Util::WrapTwoPI(Util::DegreesToRadians(theta / 240.0)); + + return theta; + +#if 0 + static const double C1 = 1.72027916940703639e-2; + static const double THGR70 = 1.7321343856509374; + static const double FK5R = 5.07551419432269442e-15; + + /* + * get integer number of days from 0 jan 1970 + */ + const double ts70 = date_ - 2433281.5 - 7305.0; + const double ds70 = floor(ts70 + 1.0e-8); + const double tfrac = ts70 - ds70; + /* + * find greenwich location at epoch + */ + const double c1p2p = C1 + kTWOPI; + double gsto = Util::WrapTwoPI(THGR70 + C1 * ds70 + c1p2p * tfrac + + ts70 * ts70 * FK5R); + + return gsto; +#endif +} + +/* + * Local Mean Sideral Time + */ +double Julian::ToLocalMeanSiderealTime(const double& lon) const +{ + return fmod(ToGreenwichSiderealTime() + lon, kTWOPI); +} + +void Julian::ToGregorian(struct DateTimeComponents* datetime) const +{ + double jdAdj = GetDate() + 0.5; + int Z = (int) jdAdj; + double F = jdAdj - Z; + + int A = 0; + + if (Z < 2299161) + { + A = static_cast (Z); + } + else + { + int a = static_cast ((Z - 1867216.25) / 36524.25); + A = static_cast (Z + 1 + a - static_cast (a / 4)); + } + + int B = A + 1524; + int C = static_cast ((B - 122.1) / 365.25); + int D = static_cast (365.25 * C); + int E = static_cast ((B - D) / 30.6001); + + datetime->hours = static_cast (F * 24.0); + F -= datetime->hours / 24.0; + datetime->minutes = static_cast (F * 1440.0); + F -= datetime->minutes / 1440.0; + datetime->seconds = F * 86400.0; + + datetime->days = B - D - static_cast (30.6001 * E); + datetime->months = E < 14 ? E - 1 : E - 13; + datetime->years = datetime->months > 2 ? C - 4716 : C - 4715; +} diff --git a/Observer.cpp b/Observer.cpp index 1541434..2a6a993 100644 --- a/Observer.cpp +++ b/Observer.cpp @@ -1,63 +1,63 @@ -#include "Observer.h" - -/* - * calculate lookangle between the observer and the passed in Eci object - */ -CoordTopographic Observer::GetLookAngle(const Eci &eci) -{ - /* - * update the observers Eci to match the time of the Eci passed in - * if necessary - */ - UpdateObserversEci(eci.GetDate()); - - /* - * calculate differences - */ - Vector range_rate = eci.GetVelocity().Subtract(observers_eci_.GetVelocity()); - Vector range = eci.GetPosition().Subtract(observers_eci_.GetPosition()); - - range.w = range.GetMagnitude(); - - /* - * Calculate Local Mean Sidereal Time for observers longitude - */ - double theta = eci.GetDate().ToLocalMeanSiderealTime(geo_.longitude); - - double sin_lat = sin(geo_.latitude); - double cos_lat = cos(geo_.latitude); - double sin_theta = sin(theta); - double cos_theta = cos(theta); - - double top_s = sin_lat * cos_theta * range.x - + sin_lat * sin_theta * range.y - cos_lat * range.z; - double top_e = -sin_theta * range.x - + cos_theta * range.y; - double top_z = cos_lat * cos_theta * range.x - + cos_lat * sin_theta * range.y + sin_lat * range.z; - double az = atan(-top_e / top_s); - - if (top_s > 0.0) - { - az += kPI; - } - - if (az < 0.0) - { - az += 2.0 * kPI; - } - - double el = asin(top_z / range.w); - double rate = range.Dot(range_rate) / range.w; - - /* - * azimuth in radians - * elevation in radians - * range in km - * range rate in km/s - */ - return CoordTopographic(az, - el, - range.w, - rate); -} +#include "Observer.h" + +/* + * calculate lookangle between the observer and the passed in Eci object + */ +CoordTopographic Observer::GetLookAngle(const Eci &eci) +{ + /* + * update the observers Eci to match the time of the Eci passed in + * if necessary + */ + UpdateObserversEci(eci.GetDate()); + + /* + * calculate differences + */ + Vector range_rate = eci.GetVelocity().Subtract(observers_eci_.GetVelocity()); + Vector range = eci.GetPosition().Subtract(observers_eci_.GetPosition()); + + range.w = range.GetMagnitude(); + + /* + * Calculate Local Mean Sidereal Time for observers longitude + */ + double theta = eci.GetDate().ToLocalMeanSiderealTime(geo_.longitude); + + double sin_lat = sin(geo_.latitude); + double cos_lat = cos(geo_.latitude); + double sin_theta = sin(theta); + double cos_theta = cos(theta); + + double top_s = sin_lat * cos_theta * range.x + + sin_lat * sin_theta * range.y - cos_lat * range.z; + double top_e = -sin_theta * range.x + + cos_theta * range.y; + double top_z = cos_lat * cos_theta * range.x + + cos_lat * sin_theta * range.y + sin_lat * range.z; + double az = atan(-top_e / top_s); + + if (top_s > 0.0) + { + az += kPI; + } + + if (az < 0.0) + { + az += 2.0 * kPI; + } + + double el = asin(top_z / range.w); + double rate = range.Dot(range_rate) / range.w; + + /* + * azimuth in radians + * elevation in radians + * range in km + * range rate in km/s + */ + return CoordTopographic(az, + el, + range.w, + rate); +} diff --git a/OrbitalElements.cpp b/OrbitalElements.cpp index 91e97bd..736f863 100644 --- a/OrbitalElements.cpp +++ b/OrbitalElements.cpp @@ -1,47 +1,47 @@ -#include "OrbitalElements.h" - -OrbitalElements::OrbitalElements(const Tle& tle) -{ - /* - * extract and format tle data - */ - mean_anomoly_ = tle.MeanAnomaly(false); - ascending_node_ = tle.RightAscendingNode(false); - argument_perigee_ = tle.ArgumentPerigee(false); - eccentricity_ = tle.Eccentricity(); - inclination_ = tle.Inclination(false); - mean_motion_ = tle.MeanMotion() * kTWOPI / kMINUTES_PER_DAY; - bstar_ = tle.BStar(); - epoch_ = tle.Epoch(); - - /* - * recover original mean motion (xnodp) and semimajor axis (aodp) - * from input elements - */ - const double a1 = pow(kXKE / MeanMotion(), kTWOTHIRD); - const double cosio = cos(Inclination()); - const double theta2 = cosio * cosio; - const double x3thm1 = 3.0 * theta2 - 1.0; - const double eosq = Eccentricity() * Eccentricity(); - const double betao2 = 1.0 - eosq; - const double betao = sqrt(betao2); - const double temp = (1.5 * kCK2) * x3thm1 / (betao * betao2); - const double del1 = temp / (a1 * a1); - const double a0 = a1 * (1.0 - del1 * (1.0 / 3.0 + del1 * (1.0 + del1 * 134.0 / 81.0))); - const double del0 = temp / (a0 * a0); - - recovered_mean_motion_ = MeanMotion() / (1.0 + del0); - /* - * alternative way to calculate - * doesnt affect final results - * recovered_semi_major_axis_ = pow(XKE / RecoveredMeanMotion(), TWOTHIRD); - */ - recovered_semi_major_axis_ = a0 / (1.0 - del0); - - /* - * find perigee and period - */ - perigee_ = (RecoveredSemiMajorAxis() * (1.0 - Eccentricity()) - kAE) * kXKMPER; - period_ = kTWOPI / RecoveredMeanMotion(); -} - +#include "OrbitalElements.h" + +OrbitalElements::OrbitalElements(const Tle& tle) +{ + /* + * extract and format tle data + */ + mean_anomoly_ = tle.MeanAnomaly(false); + ascending_node_ = tle.RightAscendingNode(false); + argument_perigee_ = tle.ArgumentPerigee(false); + eccentricity_ = tle.Eccentricity(); + inclination_ = tle.Inclination(false); + mean_motion_ = tle.MeanMotion() * kTWOPI / kMINUTES_PER_DAY; + bstar_ = tle.BStar(); + epoch_ = tle.Epoch(); + + /* + * recover original mean motion (xnodp) and semimajor axis (aodp) + * from input elements + */ + const double a1 = pow(kXKE / MeanMotion(), kTWOTHIRD); + const double cosio = cos(Inclination()); + const double theta2 = cosio * cosio; + const double x3thm1 = 3.0 * theta2 - 1.0; + const double eosq = Eccentricity() * Eccentricity(); + const double betao2 = 1.0 - eosq; + const double betao = sqrt(betao2); + const double temp = (1.5 * kCK2) * x3thm1 / (betao * betao2); + const double del1 = temp / (a1 * a1); + const double a0 = a1 * (1.0 - del1 * (1.0 / 3.0 + del1 * (1.0 + del1 * 134.0 / 81.0))); + const double del0 = temp / (a0 * a0); + + recovered_mean_motion_ = MeanMotion() / (1.0 + del0); + /* + * alternative way to calculate + * doesnt affect final results + * recovered_semi_major_axis_ = pow(XKE / RecoveredMeanMotion(), TWOTHIRD); + */ + recovered_semi_major_axis_ = a0 / (1.0 - del0); + + /* + * find perigee and period + */ + perigee_ = (RecoveredSemiMajorAxis() * (1.0 - Eccentricity()) - kAE) * kXKMPER; + period_ = kTWOPI / RecoveredMeanMotion(); +} + diff --git a/RunTest.cpp b/RunTest.cpp index 8ab56cb..84fd0a8 100644 --- a/RunTest.cpp +++ b/RunTest.cpp @@ -1,238 +1,238 @@ -#include "Julian.h" -#include "Tle.h" -#include "SGP4.h" -#include "Globals.h" -#include "Util.h" -#include "Observer.h" -#include "CoordGeodetic.h" -#include "CoordTopographic.h" - -#include -#include -#include -#include -#include -#include -#include - -void RunTle(Tle tle, double start, double end, double inc) -{ - double current = start; - SGP4 model(tle); - bool running = true; - bool first_run = true; - std::cout << " " << std::setprecision(0) << tle.NoradNumber() << " xx" << std::endl; - while (running) - { - try - { - double val; - if (first_run && current != 0.0) - { - /* - * make sure first run is always as zero - */ - val = 0.0; - } - else - { - /* - * otherwise run as normal - */ - val = current; - } - Eci eci = model.FindPosition(val); - - Vector position = eci.GetPosition(); - Vector velocity = eci.GetVelocity(); - - std::cout << std::setprecision(8) << std::fixed; - std::cout.width(17); - std::cout << val << " "; - std::cout.width(16); - std::cout << position.x << " "; - std::cout.width(16); - std::cout << position.y << " "; - std::cout.width(16); - std::cout << position.z << " "; - std::cout << std::setprecision(9) << std::fixed; - std::cout.width(14); - std::cout << velocity.x << " "; - std::cout.width(14); - std::cout << velocity.y << " "; - std::cout.width(14); - std::cout << velocity.z << std::endl; - - } - catch (SatelliteException& e) - { - std::cout << e.what() << std::endl; - running = false; - } - if ((first_run && current == 0.0) || !first_run) - { - if (current == end) - { - running = false; - } - else if (current + inc > end) - { - current = end; - } - else - { - current += inc; - } - } - first_run = false; - - } -} - -void tokenize(const std::string& str, std::vector& tokens) -{ - const std::string& delimiters = " "; - - /* - * skip delimiters at beginning - */ - std::string::size_type last_pos = str.find_first_not_of(delimiters, 0); - - /* - * find first non-delimiter - */ - std::string::size_type pos = str.find_first_of(delimiters, last_pos); - - while (std::string::npos != pos || std::string::npos != last_pos) - { - /* - * add found token to vector - */ - tokens.push_back(str.substr(last_pos, pos - last_pos)); - /* - * skip delimiters - */ - last_pos = str.find_first_not_of(delimiters, pos); - /* - * find next non-delimiter - */ - pos = str.find_first_of(delimiters, last_pos); - } -} - -void RunTest(const char* infile) -{ - std::ifstream file; - - file.open(infile); - - if (!file.is_open()) - { - std::cerr << "Error opening file" << std::endl; - return; - } - - bool got_first_line = false; - std::string line1; - std::string line2; - std::string parameters; - - while (!file.eof()) - { - std::string line; - std::getline(file, line); - - Util::Trim(line); - - /* - * skip blank lines or lines starting with # - */ - if (line.length() == 0 || line[0] == '#') - { - got_first_line = false; - continue; - } - - /* - * find first line - */ - if (!got_first_line) - { - try - { - Tle::IsValidLine(line, 1); - /* - * store line and now read in second line - */ - got_first_line = true; - line1 = line; - } - catch (TleException& e) - { - std::cerr << "Error: " << e.what() << std::endl; - std::cerr << line << std::endl; - } - } - else - { - /* - * no second chances, second line should follow the first - */ - got_first_line = false; - /* - * split line, first 69 is the second line of the tle - * the rest is the test parameters, if there is any - */ - line2 = line.substr(0, 69); - double start = 0.0; - double end = 1440.0; - double inc = 120.0; - if (line.length() > 69) - { - std::vector tokens; - parameters = line.substr(70, line.length() - 69); - tokenize(parameters, tokens); - if (tokens.size() >= 3) - { - start = atof(tokens[0].c_str()); - end = atof(tokens[1].c_str()); - inc = atof(tokens[2].c_str()); - } - } - - /* - * following line must be the second line - */ - try - { - Tle::IsValidLine(line2, 2); - Tle tle("Test", line1, line2); - RunTle(tle, start, end, inc); - } - catch (TleException& e) - { - std::cerr << "Error: " << e.what() << std::endl; - std::cerr << line << std::endl; - } - } - } - - /* - * close file - */ - file.close(); - - return; -} - -int main() -{ - const char* file_name = "SGP4-VER.TLE"; - - RunTest(file_name); - - return 1; -} - - - +#include "Julian.h" +#include "Tle.h" +#include "SGP4.h" +#include "Globals.h" +#include "Util.h" +#include "Observer.h" +#include "CoordGeodetic.h" +#include "CoordTopographic.h" + +#include +#include +#include +#include +#include +#include +#include + +void RunTle(Tle tle, double start, double end, double inc) +{ + double current = start; + SGP4 model(tle); + bool running = true; + bool first_run = true; + std::cout << " " << std::setprecision(0) << tle.NoradNumber() << " xx" << std::endl; + while (running) + { + try + { + double val; + if (first_run && current != 0.0) + { + /* + * make sure first run is always as zero + */ + val = 0.0; + } + else + { + /* + * otherwise run as normal + */ + val = current; + } + Eci eci = model.FindPosition(val); + + Vector position = eci.GetPosition(); + Vector velocity = eci.GetVelocity(); + + std::cout << std::setprecision(8) << std::fixed; + std::cout.width(17); + std::cout << val << " "; + std::cout.width(16); + std::cout << position.x << " "; + std::cout.width(16); + std::cout << position.y << " "; + std::cout.width(16); + std::cout << position.z << " "; + std::cout << std::setprecision(9) << std::fixed; + std::cout.width(14); + std::cout << velocity.x << " "; + std::cout.width(14); + std::cout << velocity.y << " "; + std::cout.width(14); + std::cout << velocity.z << std::endl; + + } + catch (SatelliteException& e) + { + std::cout << e.what() << std::endl; + running = false; + } + if ((first_run && current == 0.0) || !first_run) + { + if (current == end) + { + running = false; + } + else if (current + inc > end) + { + current = end; + } + else + { + current += inc; + } + } + first_run = false; + + } +} + +void tokenize(const std::string& str, std::vector& tokens) +{ + const std::string& delimiters = " "; + + /* + * skip delimiters at beginning + */ + std::string::size_type last_pos = str.find_first_not_of(delimiters, 0); + + /* + * find first non-delimiter + */ + std::string::size_type pos = str.find_first_of(delimiters, last_pos); + + while (std::string::npos != pos || std::string::npos != last_pos) + { + /* + * add found token to vector + */ + tokens.push_back(str.substr(last_pos, pos - last_pos)); + /* + * skip delimiters + */ + last_pos = str.find_first_not_of(delimiters, pos); + /* + * find next non-delimiter + */ + pos = str.find_first_of(delimiters, last_pos); + } +} + +void RunTest(const char* infile) +{ + std::ifstream file; + + file.open(infile); + + if (!file.is_open()) + { + std::cerr << "Error opening file" << std::endl; + return; + } + + bool got_first_line = false; + std::string line1; + std::string line2; + std::string parameters; + + while (!file.eof()) + { + std::string line; + std::getline(file, line); + + Util::Trim(line); + + /* + * skip blank lines or lines starting with # + */ + if (line.length() == 0 || line[0] == '#') + { + got_first_line = false; + continue; + } + + /* + * find first line + */ + if (!got_first_line) + { + try + { + Tle::IsValidLine(line, 1); + /* + * store line and now read in second line + */ + got_first_line = true; + line1 = line; + } + catch (TleException& e) + { + std::cerr << "Error: " << e.what() << std::endl; + std::cerr << line << std::endl; + } + } + else + { + /* + * no second chances, second line should follow the first + */ + got_first_line = false; + /* + * split line, first 69 is the second line of the tle + * the rest is the test parameters, if there is any + */ + line2 = line.substr(0, 69); + double start = 0.0; + double end = 1440.0; + double inc = 120.0; + if (line.length() > 69) + { + std::vector tokens; + parameters = line.substr(70, line.length() - 69); + tokenize(parameters, tokens); + if (tokens.size() >= 3) + { + start = atof(tokens[0].c_str()); + end = atof(tokens[1].c_str()); + inc = atof(tokens[2].c_str()); + } + } + + /* + * following line must be the second line + */ + try + { + Tle::IsValidLine(line2, 2); + Tle tle("Test", line1, line2); + RunTle(tle, start, end, inc); + } + catch (TleException& e) + { + std::cerr << "Error: " << e.what() << std::endl; + std::cerr << line << std::endl; + } + } + } + + /* + * close file + */ + file.close(); + + return; +} + +int main() +{ + const char* file_name = "SGP4-VER.TLE"; + + RunTest(file_name); + + return 1; +} + + + diff --git a/SGP4.cpp b/SGP4.cpp index 2f9b900..9264401 100644 --- a/SGP4.cpp +++ b/SGP4.cpp @@ -1,1247 +1,1247 @@ -#include "SGP4.h" - -#include "Util.h" -#include "Vector.h" -#include "SatelliteException.h" - -#include -#include - -void SGP4::SetTle(const Tle& tle) -{ - /* - * extract and format tle data - */ - elements_ = OrbitalElements(tle); - - Initialise(); -} - -void SGP4::Initialise() -{ - /* - * reset all constants etc - */ - Reset(); - - /* - * error checks - */ - if (elements_.Eccentricity() < 0.0 || elements_.Eccentricity() > 1.0 - 1.0e-3) - { - throw SatelliteException("Eccentricity out of range"); - } - - if (elements_.Inclination() < 0.0 || elements_.Inclination() > kPI) - { - throw SatelliteException("Inclination out of range"); - } - - common_consts_.cosio = cos(elements_.Inclination()); - common_consts_.sinio = sin(elements_.Inclination()); - const double theta2 = common_consts_.cosio * common_consts_.cosio; - common_consts_.x3thm1 = 3.0 * theta2 - 1.0; - const double eosq = elements_.Eccentricity() * elements_.Eccentricity(); - const double betao2 = 1.0 - eosq; - const double betao = sqrt(betao2); - - if (elements_.Period() >= 225.0) - { - use_deep_space_ = true; - } - else - { - use_deep_space_ = false; - use_simple_model_ = false; - /* - * for perigee less than 220 kilometers, the simple_model flag is set and - * the equations are truncated to linear variation in sqrt a and - * quadratic variation in mean anomly. also, the c3 term, the - * delta omega term and the delta m term are dropped - */ - if (elements_.Perigee() < 220.0) - { - use_simple_model_ = true; - } - } - - /* - * for perigee below 156km, the values of - * s4 and qoms2t are altered - */ - double s4 = kS; - double qoms24 = kQOMS2T; - if (elements_.Perigee() < 156.0) - { - s4 = elements_.Perigee() - 78.0; - if (elements_.Perigee() < 98.0) - { - s4 = 20.0; - } - qoms24 = pow((120.0 - s4) * kAE / kXKMPER, 4.0); - s4 = s4 / kXKMPER + kAE; - } - - /* - * generate constants - */ - const double pinvsq = 1.0 / (elements_.RecoveredSemiMajorAxis() * elements_.RecoveredSemiMajorAxis() * betao2 * betao2); - const double tsi = 1.0 / (elements_.RecoveredSemiMajorAxis() - s4); - common_consts_.eta = elements_.RecoveredSemiMajorAxis() * elements_.Eccentricity() * tsi; - const double etasq = common_consts_.eta * common_consts_.eta; - const double eeta = elements_.Eccentricity() * common_consts_.eta; - const double psisq = fabs(1.0 - etasq); - const double coef = qoms24 * pow(tsi, 4.0); - const double coef1 = coef / pow(psisq, 3.5); - const double c2 = coef1 * elements_.RecoveredMeanMotion() * (elements_.RecoveredSemiMajorAxis() * - (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) + - 0.75 * kCK2 * tsi / psisq * - common_consts_.x3thm1 * (8.0 + 3.0 * etasq * - (8.0 + etasq))); - common_consts_.c1 = elements_.BStar() * c2; - common_consts_.a3ovk2 = -kXJ3 / kCK2 * pow(kAE, 3.0); - common_consts_.x1mth2 = 1.0 - theta2; - common_consts_.c4 = 2.0 * elements_.RecoveredMeanMotion() * coef1 * elements_.RecoveredSemiMajorAxis() * betao2 * - (common_consts_.eta * (2.0 + 0.5 * etasq) + elements_.Eccentricity() * (0.5 + 2.0 * etasq) - - 2.0 * kCK2 * tsi / (elements_.RecoveredSemiMajorAxis() * psisq) * - (-3.0 * common_consts_.x3thm1 * (1.0 - 2.0 * eeta + etasq * - (1.5 - 0.5 * eeta)) + 0.75 * common_consts_.x1mth2 * (2.0 * etasq - eeta * - (1.0 + etasq)) * cos(2.0 * elements_.ArgumentPerigee()))); - const double theta4 = theta2 * theta2; - const double temp1 = 3.0 * kCK2 * pinvsq * elements_.RecoveredMeanMotion(); - const double temp2 = temp1 * kCK2 * pinvsq; - const double temp3 = 1.25 * kCK4 * pinvsq * pinvsq * elements_.RecoveredMeanMotion(); - common_consts_.xmdot = elements_.RecoveredMeanMotion() + 0.5 * temp1 * betao * - common_consts_.x3thm1 + 0.0625 * temp2 * betao * - (13.0 - 78.0 * theta2 + 137.0 * theta4); - const double x1m5th = 1.0 - 5.0 * theta2; - common_consts_.omgdot = -0.5 * temp1 * x1m5th + - 0.0625 * temp2 * (7.0 - 114.0 * theta2 + 395.0 * theta4) + - temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4); - const double xhdot1 = -temp1 * common_consts_.cosio; - common_consts_.xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * theta2) + 2.0 * temp3 * - (3.0 - 7.0 * theta2)) * common_consts_.cosio; - common_consts_.xnodcf = 3.5 * betao2 * xhdot1 * common_consts_.c1; - common_consts_.t2cof = 1.5 * common_consts_.c1; - - if (fabs(common_consts_.cosio + 1.0) > 1.5e-12) - { - common_consts_.xlcof = 0.125 * common_consts_.a3ovk2 * common_consts_.sinio * (3.0 + 5.0 * common_consts_.cosio) / (1.0 + common_consts_.cosio); - } - else - { - common_consts_.xlcof = 0.125 * common_consts_.a3ovk2 * common_consts_.sinio * (3.0 + 5.0 * common_consts_.cosio) / 1.5e-12; - } - - common_consts_.aycof = 0.25 * common_consts_.a3ovk2 * common_consts_.sinio; - common_consts_.x7thm1 = 7.0 * theta2 - 1.0; - - if (use_deep_space_) - { - deepspace_consts_.gsto = elements_.Epoch().ToGreenwichSiderealTime(); - - DeepSpaceInitialise(eosq, common_consts_.sinio, common_consts_.cosio, betao, - theta2, betao2, - common_consts_.xmdot, common_consts_.omgdot, common_consts_.xnodot); - } - else - { - double c3 = 0.0; - if (elements_.Eccentricity() > 1.0e-4) - { - c3 = coef * tsi * common_consts_.a3ovk2 * elements_.RecoveredMeanMotion() * kAE * - common_consts_.sinio / elements_.Eccentricity(); - } - - nearspace_consts_.c5 = 2.0 * coef1 * elements_.RecoveredSemiMajorAxis() * betao2 * (1.0 + 2.75 * - (etasq + eeta) + eeta * etasq); - nearspace_consts_.omgcof = elements_.BStar() * c3 * cos(elements_.ArgumentPerigee()); - - nearspace_consts_.xmcof = 0.0; - if (elements_.Eccentricity() > 1.0e-4) - { - nearspace_consts_.xmcof = -kTWOTHIRD * coef * elements_.BStar() * kAE / eeta; - } - - nearspace_consts_.delmo = pow(1.0 + common_consts_.eta * (cos(elements_.MeanAnomoly())), 3.0); - nearspace_consts_.sinmo = sin(elements_.MeanAnomoly()); - - if (!use_simple_model_) - { - const double c1sq = common_consts_.c1 * common_consts_.c1; - nearspace_consts_.d2 = 4.0 * elements_.RecoveredSemiMajorAxis() * tsi * c1sq; - const double temp = nearspace_consts_.d2 * tsi * common_consts_.c1 / 3.0; - nearspace_consts_.d3 = (17.0 * elements_.RecoveredSemiMajorAxis() + s4) * temp; - nearspace_consts_.d4 = 0.5 * temp * elements_.RecoveredSemiMajorAxis() * - tsi * (221.0 * elements_.RecoveredSemiMajorAxis() + 31.0 * s4) * common_consts_.c1; - nearspace_consts_.t3cof = nearspace_consts_.d2 + 2.0 * c1sq; - nearspace_consts_.t4cof = 0.25 * (3.0 * nearspace_consts_.d3 + common_consts_.c1 * - (12.0 * nearspace_consts_.d2 + 10.0 * c1sq)); - nearspace_consts_.t5cof = 0.2 * (3.0 * nearspace_consts_.d4 + 12.0 * common_consts_.c1 * - nearspace_consts_.d3 + 6.0 * nearspace_consts_.d2 * nearspace_consts_.d2 + 15.0 * - c1sq * (2.0 * nearspace_consts_.d2 + c1sq)); - } - } -} - -Eci SGP4::FindPosition(double tsince) const -{ - if (use_deep_space_) - { - return FindPositionSDP4(tsince); - } - else - { - return FindPositionSGP4(tsince); - } -} - -Eci SGP4::FindPosition(const Julian& date) const -{ - Timespan diff = date - elements_.Epoch(); - - return FindPosition(diff.GetTotalMinutes()); -} - -Eci SGP4::FindPositionSDP4(double tsince) const -{ - /* - * the final values - */ - double e; - double a; - double omega; - double xl; - double xnode; - double xincl; - - /* - * update for secular gravity and atmospheric drag - */ - double xmdf = elements_.MeanAnomoly() + common_consts_.xmdot * tsince; - double omgadf = elements_.ArgumentPerigee() + common_consts_.omgdot * tsince; - const double xnoddf = elements_.AscendingNode() + common_consts_.xnodot * tsince; - - const double tsq = tsince * tsince; - xnode = xnoddf + common_consts_.xnodcf * tsq; - double tempa = 1.0 - common_consts_.c1 * tsince; - double tempe = elements_.BStar() * common_consts_.c4 * tsince; - double templ = common_consts_.t2cof * tsq; - - double xn = elements_.RecoveredMeanMotion(); - e = elements_.Eccentricity(); - xincl = elements_.Inclination(); - - DeepSpaceSecular(tsince, &xmdf, &omgadf, &xnode, &e, &xincl, &xn); - - if (xn <= 0.0) - { - throw SatelliteException("Error: #2 (xn <= 0.0)"); - } - - a = pow(kXKE / xn, kTWOTHIRD) * pow(tempa, 2.0); - e -= tempe; - double xmam = xmdf + elements_.RecoveredMeanMotion() * templ; - - /* - * fix tolerance for error recognition - */ - if (e >= 1.0 || e < -0.001) - { - throw SatelliteException("Error: #1 (e >= 1.0 || e < -0.001)"); - } - /* - * fix tolerance to avoid a divide by zero - */ - if (e < 1.0e-6) - { - e = 1.0e-6; - } - - DeepSpacePeriodics(tsince, &e, &xincl, &omgadf, &xnode, &xmam); - - /* - * keeping xincl positive important unless you need to display xincl - * and dislike negative inclinations - */ - if (xincl < 0.0) - { - xincl = -xincl; - xnode += kPI; - omgadf -= kPI; - } - - xl = xmam + omgadf + xnode; - omega = omgadf; - - if (e < 0.0 || e > 1.0) - { - throw SatelliteException("Error: #3 (e < 0.0 || e > 1.0)"); - } - - /* - * re-compute the perturbed values - */ - const double perturbed_sinio = sin(xincl); - const double perturbed_cosio = cos(xincl); - - const double perturbed_theta2 = perturbed_cosio * perturbed_cosio; - - const double perturbed_x3thm1 = 3.0 * perturbed_theta2 - 1.0; - const double perturbed_x1mth2 = 1.0 - perturbed_theta2; - const double perturbed_x7thm1 = 7.0 * perturbed_theta2 - 1.0; - - double perturbed_xlcof; - if (fabs(perturbed_cosio + 1.0) > 1.5e-12) - { - perturbed_xlcof = 0.125 * common_consts_.a3ovk2 * perturbed_sinio * (3.0 + 5.0 * perturbed_cosio) / (1.0 + perturbed_cosio); - } - else - { - perturbed_xlcof = 0.125 * common_consts_.a3ovk2 * perturbed_sinio * (3.0 + 5.0 * perturbed_cosio) / 1.5e-12; - } - - const double perturbed_aycof = 0.25 * common_consts_.a3ovk2 * perturbed_sinio; - - /* - * using calculated values, find position and velocity - */ - return CalculateFinalPositionVelocity(tsince, e, - a, omega, xl, xnode, - xincl, perturbed_xlcof, perturbed_aycof, - perturbed_x3thm1, perturbed_x1mth2, perturbed_x7thm1, - perturbed_cosio, perturbed_sinio); - -} - -Eci SGP4::FindPositionSGP4(double tsince) const -{ - /* - * the final values - */ - double e; - double a; - double omega; - double xl; - double xnode; - double xincl; - - /* - * update for secular gravity and atmospheric drag - */ - const double xmdf = elements_.MeanAnomoly() + common_consts_.xmdot * tsince; - const double omgadf = elements_.ArgumentPerigee() + common_consts_.omgdot * tsince; - const double xnoddf = elements_.AscendingNode() + common_consts_.xnodot * tsince; - - const double tsq = tsince * tsince; - xnode = xnoddf + common_consts_.xnodcf * tsq; - double tempa = 1.0 - common_consts_.c1 * tsince; - double tempe = elements_.BStar() * common_consts_.c4 * tsince; - double templ = common_consts_.t2cof * tsq; - - xincl = elements_.Inclination(); - omega = omgadf; - double xmp = xmdf; - - if (!use_simple_model_) - { - const double delomg = nearspace_consts_.omgcof * tsince; - const double delm = nearspace_consts_.xmcof * (pow(1.0 + common_consts_.eta * cos(xmdf), 3.0) - nearspace_consts_.delmo); - const double temp = delomg + delm; - - xmp += temp; - omega -= temp; - - const double tcube = tsq * tsince; - const double tfour = tsince * tcube; - - tempa = tempa - nearspace_consts_.d2 * tsq - nearspace_consts_.d3 * tcube - nearspace_consts_.d4 * tfour; - tempe += elements_.BStar() * nearspace_consts_.c5 * (sin(xmp) - nearspace_consts_.sinmo); - templ += nearspace_consts_.t3cof * tcube + tfour * (nearspace_consts_.t4cof + tsince * nearspace_consts_.t5cof); - } - - a = elements_.RecoveredSemiMajorAxis() * pow(tempa, 2.0); - e = elements_.Eccentricity() - tempe; - xl = xmp + omega + xnode + elements_.RecoveredMeanMotion() * templ; - - if (xl <= 0.0) - { - throw SatelliteException("Error: #2 (xl <= 0.0)"); - } - - /* - * fix tolerance for error recognition - */ - if (e >= 1.0 || e < -0.001) - { - throw SatelliteException("Error: #1 (e >= 1.0 || e < -0.001)"); - } - /* - * fix tolerance to avoid a divide by zero - */ - if (e < 1.0e-6) - { - e = 1.0e-6; - } - - /* - * using calculated values, find position and velocity - * we can pass in constants from Initialise() as these dont change - */ - return CalculateFinalPositionVelocity(tsince, e, - a, omega, xl, xnode, - xincl, common_consts_.xlcof, common_consts_.aycof, - common_consts_.x3thm1, common_consts_.x1mth2, common_consts_.x7thm1, - common_consts_.cosio, common_consts_.sinio); - -} - -Eci SGP4::CalculateFinalPositionVelocity(const double& tsince, const double& e, - const double& a, const double& omega, const double& xl, const double& xnode, - const double& xincl, const double& xlcof, const double& aycof, - const double& x3thm1, const double& x1mth2, const double& x7thm1, - const double& cosio, const double& sinio) const -{ - const double beta = sqrt(1.0 - e * e); - const double xn = kXKE / pow(a, 1.5); - /* - * long period periodics - */ - const double axn = e * cos(omega); - const double temp11 = 1.0 / (a * beta * beta); - const double xll = temp11 * xlcof * axn; - const double aynl = temp11 * aycof; - const double xlt = xl + xll; - const double ayn = e * sin(omega) + aynl; - const double elsq = axn * axn + ayn * ayn; - - /* - * solve keplers equation - * - solve using Newton-Raphson root solving - * - here capu is almost the mean anomoly - * - initialise the eccentric anomaly term epw - * - The fmod saves reduction of angle to +/-2pi in sin/cos() and prevents - * convergence problems. - */ - const double capu = fmod(xlt - xnode, kTWOPI); - double epw = capu; - - double sinepw = 0.0; - double cosepw = 0.0; - double ecose = 0.0; - double esine = 0.0; - - /* - * sensibility check for N-R correction - */ - const double max_newton_naphson = 1.25 * fabs(sqrt(elsq)); - - bool kepler_running = true; - - for (int i = 0; i < 10 && kepler_running; i++) - { - sinepw = sin(epw); - cosepw = cos(epw); - ecose = axn * cosepw + ayn * sinepw; - esine = axn * sinepw - ayn * cosepw; - - double f = capu - epw + esine; - - if (fabs(f) < 1.0e-12) - { - kepler_running = false; - } - else - { - /* - * 1st order Newton-Raphson correction - */ - const double fdot = 1.0 - ecose; - double delta_epw = f / fdot; - - /* - * 2nd order Newton-Raphson correction. - * f / (fdot - 0.5 * d2f * f/fdot) - */ - if (i == 0) - { - if (delta_epw > max_newton_naphson) - { - delta_epw = max_newton_naphson; - } - else if (delta_epw < -max_newton_naphson) - { - delta_epw = -max_newton_naphson; - } - } - else - { - delta_epw = f / (fdot + 0.5 * esine * delta_epw); - } - - /* - * Newton-Raphson correction of -F/DF - */ - epw += delta_epw; - } - } - /* - * short period preliminary quantities - */ - const double temp21 = 1.0 - elsq; - const double pl = a * temp21; - - if (pl < 0.0) - { - throw SatelliteException("Error: #4 (pl < 0.0)"); - } - - const double r = a * (1.0 - ecose); - const double temp31 = 1.0 / r; - const double rdot = kXKE * sqrt(a) * esine * temp31; - const double rfdot = kXKE * sqrt(pl) * temp31; - const double temp32 = a * temp31; - const double betal = sqrt(temp21); - const double temp33 = 1.0 / (1.0 + betal); - const double cosu = temp32 * (cosepw - axn + ayn * esine * temp33); - const double sinu = temp32 * (sinepw - ayn - axn * esine * temp33); - const double u = atan2(sinu, cosu); - const double sin2u = 2.0 * sinu * cosu; - const double cos2u = 2.0 * cosu * cosu - 1.0; - - /* - * update for short periodics - */ - const double temp41 = 1.0 / pl; - const double temp42 = kCK2 * temp41; - const double temp43 = temp42 * temp41; - - const double rk = r * (1.0 - 1.5 * temp43 * betal * x3thm1) + 0.5 * temp42 * x1mth2 * cos2u; - const double uk = u - 0.25 * temp43 * x7thm1 * sin2u; - const double xnodek = xnode + 1.5 * temp43 * cosio * sin2u; - const double xinck = xincl + 1.5 * temp43 * cosio * sinio * cos2u; - const double rdotk = rdot - xn * temp42 * x1mth2 * sin2u; - const double rfdotk = rfdot + xn * temp42 * (x1mth2 * cos2u + 1.5 * x3thm1); - - if (rk < 1.0) - { - throw SatelliteException("Error: #6 Satellite decayed (rk < 1.0)"); - } - - /* - * orientation vectors - */ - const double sinuk = sin(uk); - const double cosuk = cos(uk); - const double sinik = sin(xinck); - const double cosik = cos(xinck); - const double sinnok = sin(xnodek); - const double cosnok = cos(xnodek); - const double xmx = -sinnok * cosik; - const double xmy = cosnok * cosik; - const double ux = xmx * sinuk + cosnok * cosuk; - const double uy = xmy * sinuk + sinnok * cosuk; - const double uz = sinik * sinuk; - const double vx = xmx * cosuk - cosnok * sinuk; - const double vy = xmy * cosuk - sinnok * sinuk; - const double vz = sinik * cosuk; - /* - * position and velocity - */ - const double x = rk * ux * kXKMPER; - const double y = rk * uy * kXKMPER; - const double z = rk * uz * kXKMPER; - Vector position(x, y, z); - const double xdot = (rdotk * ux + rfdotk * vx) * kXKMPER / 60.0; - const double ydot = (rdotk * uy + rfdotk * vy) * kXKMPER / 60.0; - const double zdot = (rdotk * uz + rfdotk * vz) * kXKMPER / 60.0; - Vector velocity(xdot, ydot, zdot); - - Julian julian(elements_.Epoch()); - julian.AddMin(tsince); - - Eci eci(julian, position, velocity); - - return eci; -} - -static inline double EvaluateCubicPolynomial(const double x, const double constant, - const double linear, const double squared, const double cubed) -{ - return constant + x * (linear + x * (squared + x * cubed)); -} - -/* - * deep space initialisation - */ -void SGP4::DeepSpaceInitialise(const double& eosq, const double& sinio, const double& cosio, const double& betao, - const double& theta2, const double& betao2, - const double& xmdot, const double& omgdot, const double& xnodot) -{ - double se = 0.0; - double si = 0.0; - double sl = 0.0; - double sgh = 0.0; - double shdq = 0.0; - - double bfact = 0.0; - - static const double ZNS = 1.19459E-5; - static const double C1SS = 2.9864797E-6; - static const double ZES = 0.01675; - static const double ZNL = 1.5835218E-4; - static const double C1L = 4.7968065E-7; - static const double ZEL = 0.05490; - static const double ZCOSIS = 0.91744867; - static const double ZSINI = 0.39785416; - static const double ZSINGS = -0.98088458; - static const double ZCOSGS = 0.1945905; - static const double Q22 = 1.7891679E-6; - static const double Q31 = 2.1460748E-6; - static const double Q33 = 2.2123015E-7; - static const double ROOT22 = 1.7891679E-6; - static const double ROOT32 = 3.7393792E-7; - static const double ROOT44 = 7.3636953E-9; - static const double ROOT52 = 1.1428639E-7; - static const double ROOT54 = 2.1765803E-9; - - const double aqnv = 1.0 / elements_.RecoveredSemiMajorAxis(); - const double xpidot = omgdot + xnodot; - const double sinq = sin(elements_.AscendingNode()); - const double cosq = cos(elements_.AscendingNode()); - const double sing = sin(elements_.ArgumentPerigee()); - const double cosg = cos(elements_.ArgumentPerigee()); - - /* - * initialize lunar / solar terms - */ - const double jday = elements_.Epoch().FromJan1_12h_1900(); - - const double xnodce = 4.5236020 - 9.2422029e-4 * jday; - const double xnodce_temp = fmod(xnodce, kTWOPI); - const double stem = sin(xnodce_temp); - const double ctem = cos(xnodce_temp); - const double zcosil = 0.91375164 - 0.03568096 * ctem; - const double zsinil = sqrt(1.0 - zcosil * zcosil); - const double zsinhl = 0.089683511 * stem / zsinil; - const double zcoshl = sqrt(1.0 - zsinhl * zsinhl); - const double c = 4.7199672 + 0.22997150 * jday; - const double gam = 5.8351514 + 0.0019443680 * jday; - deepspace_consts_.zmol = Util::WrapTwoPI(c - gam); - double zx = 0.39785416 * stem / zsinil; - double zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; - zx = atan2(zx, zy); - zx = fmod(gam + zx - xnodce, kTWOPI); - - const double zcosgl = cos(zx); - const double zsingl = sin(zx); - deepspace_consts_.zmos = Util::WrapTwoPI(6.2565837 + 0.017201977 * jday); - - /* - * do solar terms - */ - double zcosg = ZCOSGS; - double zsing = ZSINGS; - double zcosi = ZCOSIS; - double zsini = ZSINI; - double zcosh = cosq; - double zsinh = sinq; - double cc = C1SS; - double zn = ZNS; - double ze = ZES; - const double xnoi = 1.0 / elements_.RecoveredMeanMotion(); - - for (int cnt = 0; cnt < 2; cnt++) - { - /* - * solar terms are done a second time after lunar terms are done - */ - const double a1 = zcosg * zcosh + zsing * zcosi * zsinh; - const double a3 = -zsing * zcosh + zcosg * zcosi * zsinh; - const double a7 = -zcosg * zsinh + zsing * zcosi * zcosh; - const double a8 = zsing * zsini; - const double a9 = zsing * zsinh + zcosg * zcosi*zcosh; - const double a10 = zcosg * zsini; - const double a2 = cosio * a7 + sinio * a8; - const double a4 = cosio * a9 + sinio * a10; - const double a5 = -sinio * a7 + cosio * a8; - const double a6 = -sinio * a9 + cosio * a10; - const double x1 = a1 * cosg + a2 * sing; - const double x2 = a3 * cosg + a4 * sing; - const double x3 = -a1 * sing + a2 * cosg; - const double x4 = -a3 * sing + a4 * cosg; - const double x5 = a5 * sing; - const double x6 = a6 * sing; - const double x7 = a5 * cosg; - const double x8 = a6 * cosg; - const double z31 = 12.0 * x1 * x1 - 3. * x3 * x3; - const double z32 = 24.0 * x1 * x2 - 6. * x3 * x4; - const double z33 = 12.0 * x2 * x2 - 3. * x4 * x4; - double z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * eosq; - double z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * eosq; - double z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * eosq; - const double z11 = -6.0 * a1 * a5 + eosq * (-24. * x1 * x7 - 6. * x3 * x5); - const double z12 = -6.0 * (a1 * a6 + a3 * a5) + eosq * (-24. * (x2 * x7 + x1 * x8) - 6. * (x3 * x6 + x4 * x5)); - const double z13 = -6.0 * a3 * a6 + eosq * (-24. * x2 * x8 - 6. * x4 * x6); - const double z21 = 6.0 * a2 * a5 + eosq * (24. * x1 * x5 - 6. * x3 * x7); - const double z22 = 6.0 * (a4 * a5 + a2 * a6) + eosq * (24. * (x2 * x5 + x1 * x6) - 6. * (x4 * x7 + x3 * x8)); - const double z23 = 6.0 * a4 * a6 + eosq * (24. * x2 * x6 - 6. * x4 * x8); - z1 = z1 + z1 + betao2 * z31; - z2 = z2 + z2 + betao2 * z32; - z3 = z3 + z3 + betao2 * z33; - const double s3 = cc * xnoi; - const double s2 = -0.5 * s3 / betao; - const double s4 = s3 * betao; - const double s1 = -15.0 * elements_.Eccentricity() * s4; - const double s5 = x1 * x3 + x2 * x4; - const double s6 = x2 * x3 + x1 * x4; - const double s7 = x2 * x4 - x1 * x3; - se = s1 * zn * s5; - si = s2 * zn * (z11 + z13); - sl = -zn * s3 * (z1 + z3 - 14.0 - 6.0 * eosq); - sgh = s4 * zn * (z31 + z33 - 6.0); - - /* - * replaced - * sh = -zn * s2 * (z21 + z23 - * with - * shdq = (-zn * s2 * (z21 + z23)) / sinio - */ - if (elements_.Inclination() < 5.2359877e-2 || elements_.Inclination() > kPI - 5.2359877e-2) - { - shdq = 0.0; - } - else - { - shdq = (-zn * s2 * (z21 + z23)) / sinio; - } - - deepspace_consts_.ee2 = 2.0 * s1 * s6; - deepspace_consts_.e3 = 2.0 * s1 * s7; - deepspace_consts_.xi2 = 2.0 * s2 * z12; - deepspace_consts_.xi3 = 2.0 * s2 * (z13 - z11); - deepspace_consts_.xl2 = -2.0 * s3 * z2; - deepspace_consts_.xl3 = -2.0 * s3 * (z3 - z1); - deepspace_consts_.xl4 = -2.0 * s3 * (-21.0 - 9.0 * eosq) * ze; - deepspace_consts_.xgh2 = 2.0 * s4 * z32; - deepspace_consts_.xgh3 = 2.0 * s4 * (z33 - z31); - deepspace_consts_.xgh4 = -18.0 * s4 * ze; - deepspace_consts_.xh2 = -2.0 * s2 * z22; - deepspace_consts_.xh3 = -2.0 * s2 * (z23 - z21); - - if (cnt == 1) - { - break; - } - /* - * do lunar terms - */ - deepspace_consts_.sse = se; - deepspace_consts_.ssi = si; - deepspace_consts_.ssl = sl; - deepspace_consts_.ssh = shdq; - deepspace_consts_.ssg = sgh - cosio * deepspace_consts_.ssh; - deepspace_consts_.se2 = deepspace_consts_.ee2; - deepspace_consts_.si2 = deepspace_consts_.xi2; - deepspace_consts_.sl2 = deepspace_consts_.xl2; - deepspace_consts_.sgh2 = deepspace_consts_.xgh2; - deepspace_consts_.sh2 = deepspace_consts_.xh2; - deepspace_consts_.se3 = deepspace_consts_.e3; - deepspace_consts_.si3 = deepspace_consts_.xi3; - deepspace_consts_.sl3 = deepspace_consts_.xl3; - deepspace_consts_.sgh3 = deepspace_consts_.xgh3; - deepspace_consts_.sh3 = deepspace_consts_.xh3; - deepspace_consts_.sl4 = deepspace_consts_.xl4; - deepspace_consts_.sgh4 = deepspace_consts_.xgh4; - zcosg = zcosgl; - zsing = zsingl; - zcosi = zcosil; - zsini = zsinil; - zcosh = zcoshl * cosq + zsinhl * sinq; - zsinh = sinq * zcoshl - cosq * zsinhl; - zn = ZNL; - cc = C1L; - ze = ZEL; - } - - deepspace_consts_.sse += se; - deepspace_consts_.ssi += si; - deepspace_consts_.ssl += sl; - deepspace_consts_.ssg += sgh - cosio * shdq; - deepspace_consts_.ssh += shdq; - - deepspace_consts_.resonance_flag = false; - deepspace_consts_.synchronous_flag = false; - bool initialise_integrator = true; - - if (elements_.RecoveredMeanMotion() < 0.0052359877 && elements_.RecoveredMeanMotion() > 0.0034906585) - { - /* - * 24h synchronous resonance terms initialisation - */ - deepspace_consts_.resonance_flag = true; - deepspace_consts_.synchronous_flag = true; - - const double g200 = 1.0 + eosq * (-2.5 + 0.8125 * eosq); - const double g310 = 1.0 + 2.0 * eosq; - const double g300 = 1.0 + eosq * (-6.0 + 6.60937 * eosq); - const double f220 = 0.75 * (1.0 + cosio) * (1.0 + cosio); - const double f311 = 0.9375 * sinio * sinio * (1.0 + 3.0 * cosio) - 0.75 * (1.0 + cosio); - double f330 = 1.0 + cosio; - f330 = 1.875 * f330 * f330 * f330; - deepspace_consts_.del1 = 3.0 * elements_.RecoveredMeanMotion() * elements_.RecoveredMeanMotion() * aqnv * aqnv; - deepspace_consts_.del2 = 2.0 * deepspace_consts_.del1 * f220 * g200 * Q22; - deepspace_consts_.del3 = 3.0 * deepspace_consts_.del1 * f330 * g300 * Q33 * aqnv; - deepspace_consts_.del1 = deepspace_consts_.del1 * f311 * g310 * Q31 * aqnv; - - integrator_consts_.xlamo = elements_.MeanAnomoly() + elements_.AscendingNode() + elements_.ArgumentPerigee() - deepspace_consts_.gsto; - bfact = xmdot + xpidot - kTHDT; - bfact += deepspace_consts_.ssl + deepspace_consts_.ssg + deepspace_consts_.ssh; - - } - else if (elements_.RecoveredMeanMotion() < 8.26e-3 || elements_.RecoveredMeanMotion() > 9.24e-3 || elements_.Eccentricity() < 0.5) - { - initialise_integrator = false; - } - else - { - /* - * geopotential resonance initialisation for 12 hour orbits - */ - deepspace_consts_.resonance_flag = true; - - double g211; - double g310; - double g322; - double g410; - double g422; - double g520; - - double g201 = -0.306 - (elements_.Eccentricity() - 0.64) * 0.440; - - if (elements_.Eccentricity() <= 0.65) - { - g211 = EvaluateCubicPolynomial(elements_.Eccentricity(), 3.616, -13.247, +16.290, 0.0); - g310 = EvaluateCubicPolynomial(elements_.Eccentricity(), -19.302, 117.390, -228.419, 156.591); - g322 = EvaluateCubicPolynomial(elements_.Eccentricity(), -18.9068, 109.7927, -214.6334, 146.5816); - g410 = EvaluateCubicPolynomial(elements_.Eccentricity(), -41.122, 242.694, -471.094, 313.953); - g422 = EvaluateCubicPolynomial(elements_.Eccentricity(), -146.407, 841.880, -1629.014, 1083.435); - g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), -532.114, 3017.977, -5740.032, 3708.276); - } - else - { - g211 = EvaluateCubicPolynomial(elements_.Eccentricity(), -72.099, 331.819, -508.738, 266.724); - g310 = EvaluateCubicPolynomial(elements_.Eccentricity(), -346.844, 1582.851, -2415.925, 1246.113); - g322 = EvaluateCubicPolynomial(elements_.Eccentricity(), -342.585, 1554.908, -2366.899, 1215.972); - g410 = EvaluateCubicPolynomial(elements_.Eccentricity(), -1052.797, 4758.686, -7193.992, 3651.957); - g422 = EvaluateCubicPolynomial(elements_.Eccentricity(), -3581.69, 16178.11, -24462.77, 12422.52); - - if (elements_.Eccentricity() <= 0.715) - { - g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), 1464.74, -4664.75, 3763.64, 0.0); - } - else - { - g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), -5149.66, 29936.92, -54087.36, 31324.56); - } - } - - double g533; - double g521; - double g532; - - if (elements_.Eccentricity() < 0.7) - { - g533 = EvaluateCubicPolynomial(elements_.Eccentricity(), -919.2277, 4988.61, -9064.77, 5542.21); - g521 = EvaluateCubicPolynomial(elements_.Eccentricity(), -822.71072, 4568.6173, -8491.4146, 5337.524); - g532 = EvaluateCubicPolynomial(elements_.Eccentricity(), -853.666, 4690.25, -8624.77, 5341.4); - } - else - { - g533 = EvaluateCubicPolynomial(elements_.Eccentricity(), -37995.78, 161616.52, -229838.2, 109377.94); - g521 = EvaluateCubicPolynomial(elements_.Eccentricity(), -51752.104, 218913.95, -309468.16, 146349.42); - g532 = EvaluateCubicPolynomial(elements_.Eccentricity(), -40023.88, 170470.89, -242699.48, 115605.82); - } - - const double sini2 = sinio * sinio; - const double f220 = 0.75 * (1.0 + 2.0 * cosio + theta2); - const double f221 = 1.5 * sini2; - const double f321 = 1.875 * sinio * (1.0 - 2.0 * cosio - 3.0 * theta2); - const double f322 = -1.875 * sinio * (1.0 + 2.0 * cosio - 3.0 * theta2); - const double f441 = 35.0 * sini2 * f220; - const double f442 = 39.3750 * sini2 * sini2; - const double f522 = 9.84375 * sinio * (sini2 * (1.0 - 2.0 * cosio - 5.0 * theta2) - + 0.33333333 * (-2.0 + 4.0 * cosio + 6.0 * theta2)); - const double f523 = sinio * (4.92187512 * sini2 * (-2.0 - 4.0 * cosio + 10.0 * theta2) - + 6.56250012 * (1.0 + 2.0 * cosio - 3.0 * theta2)); - const double f542 = 29.53125 * sinio * (2.0 - 8.0 * cosio + theta2 * - (-12.0 + 8.0 * cosio + 10.0 * theta2)); - const double f543 = 29.53125 * sinio * (-2.0 - 8.0 * cosio + theta2 * - (12.0 + 8.0 * cosio - 10.0 * theta2)); - - const double xno2 = elements_.RecoveredMeanMotion() * elements_.RecoveredMeanMotion(); - const double ainv2 = aqnv * aqnv; - - double temp1 = 3.0 * xno2 * ainv2; - double temp = temp1 * ROOT22; - deepspace_consts_.d2201 = temp * f220 * g201; - deepspace_consts_.d2211 = temp * f221 * g211; - temp1 = temp1 * aqnv; - temp = temp1 * ROOT32; - deepspace_consts_.d3210 = temp * f321 * g310; - deepspace_consts_.d3222 = temp * f322 * g322; - temp1 = temp1 * aqnv; - temp = 2.0 * temp1 * ROOT44; - deepspace_consts_.d4410 = temp * f441 * g410; - deepspace_consts_.d4422 = temp * f442 * g422; - temp1 = temp1 * aqnv; - temp = temp1 * ROOT52; - deepspace_consts_.d5220 = temp * f522 * g520; - deepspace_consts_.d5232 = temp * f523 * g532; - temp = 2.0 * temp1 * ROOT54; - deepspace_consts_.d5421 = temp * f542 * g521; - deepspace_consts_.d5433 = temp * f543 * g533; - - integrator_consts_.xlamo = elements_.MeanAnomoly() + elements_.AscendingNode() + elements_.AscendingNode() - deepspace_consts_.gsto - deepspace_consts_.gsto; - bfact = xmdot + xnodot + xnodot - kTHDT - kTHDT; - bfact = bfact + deepspace_consts_.ssl + deepspace_consts_.ssh + deepspace_consts_.ssh; - } - - if (initialise_integrator) - { - /* - * initialise integrator - */ - integrator_consts_.xfact = bfact - elements_.RecoveredMeanMotion(); - integrator_params_.atime = 0.0; - integrator_params_.xni = elements_.RecoveredMeanMotion(); - integrator_params_.xli = integrator_consts_.xlamo; - /* - * precompute dot terms for epoch - */ - DeepSpaceCalcDotTerms(&integrator_consts_.values_0); - } -} - -void SGP4::DeepSpaceCalculateLunarSolarTerms(const double t, double* pe, double* pinc, - double* pl, double* pgh, double* ph) const -{ - static const double ZES = 0.01675; - static const double ZNS = 1.19459E-5; - static const double ZNL = 1.5835218E-4; - static const double ZEL = 0.05490; - - /* - * calculate solar terms for time t - */ - double zm = deepspace_consts_.zmos + ZNS * t; - double zf = zm + 2.0 * ZES * sin(zm); - double sinzf = sin(zf); - double f2 = 0.5 * sinzf * sinzf - 0.25; - double f3 = -0.5 * sinzf * cos(zf); - const double ses = deepspace_consts_.se2 * f2 + deepspace_consts_.se3 * f3; - const double sis = deepspace_consts_.si2 * f2 + deepspace_consts_.si3 * f3; - const double sls = deepspace_consts_.sl2 * f2 + deepspace_consts_.sl3 * f3 + deepspace_consts_.sl4 * sinzf; - const double sghs = deepspace_consts_.sgh2 * f2 + deepspace_consts_.sgh3 * f3 + deepspace_consts_.sgh4 * sinzf; - const double shs = deepspace_consts_.sh2 * f2 + deepspace_consts_.sh3 * f3; - - /* - * calculate lunar terms for time t - */ - zm = deepspace_consts_.zmol + ZNL * t; - zf = zm + 2.0 * ZEL * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - const double sel = deepspace_consts_.ee2 * f2 + deepspace_consts_.e3 * f3; - const double sil = deepspace_consts_.xi2 * f2 + deepspace_consts_.xi3 * f3; - const double sll = deepspace_consts_.xl2 * f2 + deepspace_consts_.xl3 * f3 + deepspace_consts_.xl4 * sinzf; - const double sghl = deepspace_consts_.xgh2 * f2 + deepspace_consts_.xgh3 * f3 + deepspace_consts_.xgh4 * sinzf; - const double shl = deepspace_consts_.xh2 * f2 + deepspace_consts_.xh3 * f3; - - /* - * merge calculated values - */ - (*pe) = ses + sel; - (*pinc) = sis + sil; - (*pl) = sls + sll; - (*pgh) = sghs + sghl; - (*ph) = shs + shl; -} - -/* - * calculate lunar / solar periodics and apply - */ -void SGP4::DeepSpacePeriodics(const double& t, double* em, - double* xinc, double* omgasm, double* xnodes, double* xll) const -{ - /* - * storage for lunar / solar terms set by DeepSpaceCalculateLunarSolarTerms() - */ - double pe = 0.0; - double pinc = 0.0; - double pl = 0.0; - double pgh = 0.0; - double ph = 0.0; - - /* - * calculate lunar / solar terms for current time - */ - DeepSpaceCalculateLunarSolarTerms(t, &pe, &pinc, &pl, &pgh, &ph); - - (*xinc) += pinc; - (*em) += pe; - - /* Spacetrack report #3 has sin/cos from before perturbations - * added to xinc (oldxinc), but apparently report # 6 has then - * from after they are added. - * use for strn3 - * if (elements_.Inclination() >= 0.2) - * use for gsfc - * if (xinc >= 0.2) - * (moved from start of function) - */ - const double sinis = sin(*xinc); - const double cosis = cos(*xinc); - - if ((*xinc) >= 0.2) - { - /* - * apply periodics directly - */ - const double tmp_ph = ph / sinis; - - (*omgasm) += pgh - cosis * tmp_ph; - (*xnodes) += tmp_ph; - (*xll) += pl; - } - else - { - /* - * apply periodics with lyddane modification - */ - const double sinok = sin(*xnodes); - const double cosok = cos(*xnodes); - double alfdp = sinis * sinok; - double betdp = sinis * cosok; - const double dalf = ph * cosok + pinc * cosis * sinok; - const double dbet = -ph * sinok + pinc * cosis * cosok; - - alfdp += dalf; - betdp += dbet; - - (*xnodes) = Util::WrapTwoPI((*xnodes)); - - double xls = (*xll) + (*omgasm) + cosis * (*xnodes); - double dls = pl + pgh - pinc * (*xnodes) * sinis; - xls += dls; - - /* - * save old xnodes value - */ - const double oldxnodes = (*xnodes); - - (*xnodes) = atan2(alfdp, betdp); - if ((*xnodes) < 0.0) - { - (*xnodes) += kTWOPI; - } - - /* - * Get perturbed xnodes in to same quadrant as original. - * RAAN is in the range of 0 to 360 degrees - * atan2 is in the range of -180 to 180 degrees - */ - if (fabs(oldxnodes - (*xnodes)) > kPI) - { - if ((*xnodes) < oldxnodes) - { - (*xnodes) += kTWOPI; - } - else - { - (*xnodes) -= kTWOPI; - } - } - - (*xll) += pl; - (*omgasm) = xls - (*xll) - cosis * (*xnodes); - } -} - -/* - * deep space secular effects - */ -void SGP4::DeepSpaceSecular(const double& t, double* xll, double* omgasm, - double* xnodes, double* em, double* xinc, double* xn) const -{ - static const double STEP = 720.0; - static const double STEP2 = 259200.0; - - (*xll) += deepspace_consts_.ssl * t; - (*omgasm) += deepspace_consts_.ssg * t; - (*xnodes) += deepspace_consts_.ssh * t; - (*em) += deepspace_consts_.sse * t; - (*xinc) += deepspace_consts_.ssi * t; - - if (!deepspace_consts_.resonance_flag) - { - return; - } - - /* - * 1st condition (if t is less than one time step from epoch) - * 2nd condition (if integrator_params_.atime and t are of opposite signs, so zero crossing required) - * 3rd condition (if t is closer to zero than integrator_params_.atime, only integrate away from zero) - */ - if (fabs(t) < STEP || - t * integrator_params_.atime <= 0.0 || - fabs(t) < fabs(integrator_params_.atime)) - { - /* - * restart from epoch - */ - integrator_params_.atime = 0.0; - integrator_params_.xni = elements_.RecoveredMeanMotion(); - integrator_params_.xli = integrator_consts_.xlamo; - - /* - * restore precomputed values for epoch - */ - integrator_params_.values_t = integrator_consts_.values_0; - } - - double ft = t - integrator_params_.atime; - - /* - * if time difference (ft) is greater than the time step (720.0) - * loop around until integrator_params_.atime is within one time step of t - */ - if (fabs(ft) >= STEP) - { - /* - * calculate step direction to allow integrator_params_.atime - * to catch up with t - */ - double delt = -STEP; - if (ft >= 0.0) - { - delt = STEP; - } - - do - { - /* - * integrate using current dot terms - */ - DeepSpaceIntegrator(delt, STEP2, integrator_params_.values_t); - - /* - * calculate dot terms for next integration - */ - DeepSpaceCalcDotTerms(&integrator_params_.values_t); - - ft = t - integrator_params_.atime; - } while (fabs(ft) >= STEP); - } - - /* - * integrator - */ - (*xn) = integrator_params_.xni + integrator_params_.values_t.xndot * ft + integrator_params_.values_t.xnddt * ft * ft * 0.5; - const double xl = integrator_params_.xli + integrator_params_.values_t.xldot * ft + integrator_params_.values_t.xndot * ft * ft * 0.5; - const double temp = -(*xnodes) + deepspace_consts_.gsto + t * kTHDT; - - if (deepspace_consts_.synchronous_flag) - { - (*xll) = xl + temp - (*omgasm); - } - else - { - (*xll) = xl + temp + temp; - } -} - -/* - * calculate dot terms - */ -void SGP4::DeepSpaceCalcDotTerms(struct IntegratorValues *values) const -{ - static const double G22 = 5.7686396; - static const double G32 = 0.95240898; - static const double G44 = 1.8014998; - static const double G52 = 1.0508330; - static const double G54 = 4.4108898; - static const double FASX2 = 0.13130908; - static const double FASX4 = 2.8843198; - static const double FASX6 = 0.37448087; - - if (deepspace_consts_.synchronous_flag) - { - - values->xndot = deepspace_consts_.del1 * sin(integrator_params_.xli - FASX2) + - deepspace_consts_.del2 * sin(2.0 * (integrator_params_.xli - FASX4)) + - deepspace_consts_.del3 * sin(3.0 * (integrator_params_.xli - FASX6)); - values->xnddt = deepspace_consts_.del1 * cos(integrator_params_.xli - FASX2) + 2.0 * - deepspace_consts_.del2 * cos(2.0 * (integrator_params_.xli - FASX4)) + 3.0 * - deepspace_consts_.del3 * cos(3.0 * (integrator_params_.xli - FASX6)); - - } - else - { - const double xomi = elements_.ArgumentPerigee() + common_consts_.omgdot * integrator_params_.atime; - const double x2omi = xomi + xomi; - const double x2li = integrator_params_.xli + integrator_params_.xli; - - values->xndot = deepspace_consts_.d2201 * sin(x2omi + integrator_params_.xli - G22) - + deepspace_consts_.d2211 * sin(integrator_params_.xli - G22) - + deepspace_consts_.d3210 * sin(xomi + integrator_params_.xli - G32) - + deepspace_consts_.d3222 * sin(-xomi + integrator_params_.xli - G32) - + deepspace_consts_.d4410 * sin(x2omi + x2li - G44) - + deepspace_consts_.d4422 * sin(x2li - G44) - + deepspace_consts_.d5220 * sin(xomi + integrator_params_.xli - G52) - + deepspace_consts_.d5232 * sin(-xomi + integrator_params_.xli - G52) - + deepspace_consts_.d5421 * sin(xomi + x2li - G54) - + deepspace_consts_.d5433 * sin(-xomi + x2li - G54); - values->xnddt = deepspace_consts_.d2201 * cos(x2omi + integrator_params_.xli - G22) - + deepspace_consts_.d2211 * cos(integrator_params_.xli - G22) - + deepspace_consts_.d3210 * cos(xomi + integrator_params_.xli - G32) - + deepspace_consts_.d3222 * cos(-xomi + integrator_params_.xli - G32) - + deepspace_consts_.d5220 * cos(xomi + integrator_params_.xli - G52) - + deepspace_consts_.d5232 * cos(-xomi + integrator_params_.xli - G52) - + 2.0 * (deepspace_consts_.d4410 * cos(x2omi + x2li - G44) - + deepspace_consts_.d4422 * cos(x2li - G44) - + deepspace_consts_.d5421 * cos(xomi + x2li - G54) - + deepspace_consts_.d5433 * cos(-xomi + x2li - G54)); - } - - values->xldot = integrator_params_.xni + integrator_consts_.xfact; - values->xnddt *= values->xldot; -} - -/* - * deep space integrator for time period of delt - */ -void SGP4::DeepSpaceIntegrator(const double delt, const double step2, - const struct IntegratorValues &values) const -{ - /* - * integrator - */ - integrator_params_.xli += values.xldot * delt + values.xndot * step2; - integrator_params_.xni += values.xndot * delt + values.xnddt * step2; - - /* - * increment integrator time - */ - integrator_params_.atime += delt; -} - -void SGP4::Reset() -{ - /* - * common variables - */ - use_simple_model_ = false; - use_deep_space_ = false; -} +#include "SGP4.h" + +#include "Util.h" +#include "Vector.h" +#include "SatelliteException.h" + +#include +#include + +void SGP4::SetTle(const Tle& tle) +{ + /* + * extract and format tle data + */ + elements_ = OrbitalElements(tle); + + Initialise(); +} + +void SGP4::Initialise() +{ + /* + * reset all constants etc + */ + Reset(); + + /* + * error checks + */ + if (elements_.Eccentricity() < 0.0 || elements_.Eccentricity() > 1.0 - 1.0e-3) + { + throw SatelliteException("Eccentricity out of range"); + } + + if (elements_.Inclination() < 0.0 || elements_.Inclination() > kPI) + { + throw SatelliteException("Inclination out of range"); + } + + common_consts_.cosio = cos(elements_.Inclination()); + common_consts_.sinio = sin(elements_.Inclination()); + const double theta2 = common_consts_.cosio * common_consts_.cosio; + common_consts_.x3thm1 = 3.0 * theta2 - 1.0; + const double eosq = elements_.Eccentricity() * elements_.Eccentricity(); + const double betao2 = 1.0 - eosq; + const double betao = sqrt(betao2); + + if (elements_.Period() >= 225.0) + { + use_deep_space_ = true; + } + else + { + use_deep_space_ = false; + use_simple_model_ = false; + /* + * for perigee less than 220 kilometers, the simple_model flag is set and + * the equations are truncated to linear variation in sqrt a and + * quadratic variation in mean anomly. also, the c3 term, the + * delta omega term and the delta m term are dropped + */ + if (elements_.Perigee() < 220.0) + { + use_simple_model_ = true; + } + } + + /* + * for perigee below 156km, the values of + * s4 and qoms2t are altered + */ + double s4 = kS; + double qoms24 = kQOMS2T; + if (elements_.Perigee() < 156.0) + { + s4 = elements_.Perigee() - 78.0; + if (elements_.Perigee() < 98.0) + { + s4 = 20.0; + } + qoms24 = pow((120.0 - s4) * kAE / kXKMPER, 4.0); + s4 = s4 / kXKMPER + kAE; + } + + /* + * generate constants + */ + const double pinvsq = 1.0 / (elements_.RecoveredSemiMajorAxis() * elements_.RecoveredSemiMajorAxis() * betao2 * betao2); + const double tsi = 1.0 / (elements_.RecoveredSemiMajorAxis() - s4); + common_consts_.eta = elements_.RecoveredSemiMajorAxis() * elements_.Eccentricity() * tsi; + const double etasq = common_consts_.eta * common_consts_.eta; + const double eeta = elements_.Eccentricity() * common_consts_.eta; + const double psisq = fabs(1.0 - etasq); + const double coef = qoms24 * pow(tsi, 4.0); + const double coef1 = coef / pow(psisq, 3.5); + const double c2 = coef1 * elements_.RecoveredMeanMotion() * (elements_.RecoveredSemiMajorAxis() * + (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) + + 0.75 * kCK2 * tsi / psisq * + common_consts_.x3thm1 * (8.0 + 3.0 * etasq * + (8.0 + etasq))); + common_consts_.c1 = elements_.BStar() * c2; + common_consts_.a3ovk2 = -kXJ3 / kCK2 * pow(kAE, 3.0); + common_consts_.x1mth2 = 1.0 - theta2; + common_consts_.c4 = 2.0 * elements_.RecoveredMeanMotion() * coef1 * elements_.RecoveredSemiMajorAxis() * betao2 * + (common_consts_.eta * (2.0 + 0.5 * etasq) + elements_.Eccentricity() * (0.5 + 2.0 * etasq) - + 2.0 * kCK2 * tsi / (elements_.RecoveredSemiMajorAxis() * psisq) * + (-3.0 * common_consts_.x3thm1 * (1.0 - 2.0 * eeta + etasq * + (1.5 - 0.5 * eeta)) + 0.75 * common_consts_.x1mth2 * (2.0 * etasq - eeta * + (1.0 + etasq)) * cos(2.0 * elements_.ArgumentPerigee()))); + const double theta4 = theta2 * theta2; + const double temp1 = 3.0 * kCK2 * pinvsq * elements_.RecoveredMeanMotion(); + const double temp2 = temp1 * kCK2 * pinvsq; + const double temp3 = 1.25 * kCK4 * pinvsq * pinvsq * elements_.RecoveredMeanMotion(); + common_consts_.xmdot = elements_.RecoveredMeanMotion() + 0.5 * temp1 * betao * + common_consts_.x3thm1 + 0.0625 * temp2 * betao * + (13.0 - 78.0 * theta2 + 137.0 * theta4); + const double x1m5th = 1.0 - 5.0 * theta2; + common_consts_.omgdot = -0.5 * temp1 * x1m5th + + 0.0625 * temp2 * (7.0 - 114.0 * theta2 + 395.0 * theta4) + + temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4); + const double xhdot1 = -temp1 * common_consts_.cosio; + common_consts_.xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * theta2) + 2.0 * temp3 * + (3.0 - 7.0 * theta2)) * common_consts_.cosio; + common_consts_.xnodcf = 3.5 * betao2 * xhdot1 * common_consts_.c1; + common_consts_.t2cof = 1.5 * common_consts_.c1; + + if (fabs(common_consts_.cosio + 1.0) > 1.5e-12) + { + common_consts_.xlcof = 0.125 * common_consts_.a3ovk2 * common_consts_.sinio * (3.0 + 5.0 * common_consts_.cosio) / (1.0 + common_consts_.cosio); + } + else + { + common_consts_.xlcof = 0.125 * common_consts_.a3ovk2 * common_consts_.sinio * (3.0 + 5.0 * common_consts_.cosio) / 1.5e-12; + } + + common_consts_.aycof = 0.25 * common_consts_.a3ovk2 * common_consts_.sinio; + common_consts_.x7thm1 = 7.0 * theta2 - 1.0; + + if (use_deep_space_) + { + deepspace_consts_.gsto = elements_.Epoch().ToGreenwichSiderealTime(); + + DeepSpaceInitialise(eosq, common_consts_.sinio, common_consts_.cosio, betao, + theta2, betao2, + common_consts_.xmdot, common_consts_.omgdot, common_consts_.xnodot); + } + else + { + double c3 = 0.0; + if (elements_.Eccentricity() > 1.0e-4) + { + c3 = coef * tsi * common_consts_.a3ovk2 * elements_.RecoveredMeanMotion() * kAE * + common_consts_.sinio / elements_.Eccentricity(); + } + + nearspace_consts_.c5 = 2.0 * coef1 * elements_.RecoveredSemiMajorAxis() * betao2 * (1.0 + 2.75 * + (etasq + eeta) + eeta * etasq); + nearspace_consts_.omgcof = elements_.BStar() * c3 * cos(elements_.ArgumentPerigee()); + + nearspace_consts_.xmcof = 0.0; + if (elements_.Eccentricity() > 1.0e-4) + { + nearspace_consts_.xmcof = -kTWOTHIRD * coef * elements_.BStar() * kAE / eeta; + } + + nearspace_consts_.delmo = pow(1.0 + common_consts_.eta * (cos(elements_.MeanAnomoly())), 3.0); + nearspace_consts_.sinmo = sin(elements_.MeanAnomoly()); + + if (!use_simple_model_) + { + const double c1sq = common_consts_.c1 * common_consts_.c1; + nearspace_consts_.d2 = 4.0 * elements_.RecoveredSemiMajorAxis() * tsi * c1sq; + const double temp = nearspace_consts_.d2 * tsi * common_consts_.c1 / 3.0; + nearspace_consts_.d3 = (17.0 * elements_.RecoveredSemiMajorAxis() + s4) * temp; + nearspace_consts_.d4 = 0.5 * temp * elements_.RecoveredSemiMajorAxis() * + tsi * (221.0 * elements_.RecoveredSemiMajorAxis() + 31.0 * s4) * common_consts_.c1; + nearspace_consts_.t3cof = nearspace_consts_.d2 + 2.0 * c1sq; + nearspace_consts_.t4cof = 0.25 * (3.0 * nearspace_consts_.d3 + common_consts_.c1 * + (12.0 * nearspace_consts_.d2 + 10.0 * c1sq)); + nearspace_consts_.t5cof = 0.2 * (3.0 * nearspace_consts_.d4 + 12.0 * common_consts_.c1 * + nearspace_consts_.d3 + 6.0 * nearspace_consts_.d2 * nearspace_consts_.d2 + 15.0 * + c1sq * (2.0 * nearspace_consts_.d2 + c1sq)); + } + } +} + +Eci SGP4::FindPosition(double tsince) const +{ + if (use_deep_space_) + { + return FindPositionSDP4(tsince); + } + else + { + return FindPositionSGP4(tsince); + } +} + +Eci SGP4::FindPosition(const Julian& date) const +{ + Timespan diff = date - elements_.Epoch(); + + return FindPosition(diff.GetTotalMinutes()); +} + +Eci SGP4::FindPositionSDP4(double tsince) const +{ + /* + * the final values + */ + double e; + double a; + double omega; + double xl; + double xnode; + double xincl; + + /* + * update for secular gravity and atmospheric drag + */ + double xmdf = elements_.MeanAnomoly() + common_consts_.xmdot * tsince; + double omgadf = elements_.ArgumentPerigee() + common_consts_.omgdot * tsince; + const double xnoddf = elements_.AscendingNode() + common_consts_.xnodot * tsince; + + const double tsq = tsince * tsince; + xnode = xnoddf + common_consts_.xnodcf * tsq; + double tempa = 1.0 - common_consts_.c1 * tsince; + double tempe = elements_.BStar() * common_consts_.c4 * tsince; + double templ = common_consts_.t2cof * tsq; + + double xn = elements_.RecoveredMeanMotion(); + e = elements_.Eccentricity(); + xincl = elements_.Inclination(); + + DeepSpaceSecular(tsince, &xmdf, &omgadf, &xnode, &e, &xincl, &xn); + + if (xn <= 0.0) + { + throw SatelliteException("Error: #2 (xn <= 0.0)"); + } + + a = pow(kXKE / xn, kTWOTHIRD) * pow(tempa, 2.0); + e -= tempe; + double xmam = xmdf + elements_.RecoveredMeanMotion() * templ; + + /* + * fix tolerance for error recognition + */ + if (e >= 1.0 || e < -0.001) + { + throw SatelliteException("Error: #1 (e >= 1.0 || e < -0.001)"); + } + /* + * fix tolerance to avoid a divide by zero + */ + if (e < 1.0e-6) + { + e = 1.0e-6; + } + + DeepSpacePeriodics(tsince, &e, &xincl, &omgadf, &xnode, &xmam); + + /* + * keeping xincl positive important unless you need to display xincl + * and dislike negative inclinations + */ + if (xincl < 0.0) + { + xincl = -xincl; + xnode += kPI; + omgadf -= kPI; + } + + xl = xmam + omgadf + xnode; + omega = omgadf; + + if (e < 0.0 || e > 1.0) + { + throw SatelliteException("Error: #3 (e < 0.0 || e > 1.0)"); + } + + /* + * re-compute the perturbed values + */ + const double perturbed_sinio = sin(xincl); + const double perturbed_cosio = cos(xincl); + + const double perturbed_theta2 = perturbed_cosio * perturbed_cosio; + + const double perturbed_x3thm1 = 3.0 * perturbed_theta2 - 1.0; + const double perturbed_x1mth2 = 1.0 - perturbed_theta2; + const double perturbed_x7thm1 = 7.0 * perturbed_theta2 - 1.0; + + double perturbed_xlcof; + if (fabs(perturbed_cosio + 1.0) > 1.5e-12) + { + perturbed_xlcof = 0.125 * common_consts_.a3ovk2 * perturbed_sinio * (3.0 + 5.0 * perturbed_cosio) / (1.0 + perturbed_cosio); + } + else + { + perturbed_xlcof = 0.125 * common_consts_.a3ovk2 * perturbed_sinio * (3.0 + 5.0 * perturbed_cosio) / 1.5e-12; + } + + const double perturbed_aycof = 0.25 * common_consts_.a3ovk2 * perturbed_sinio; + + /* + * using calculated values, find position and velocity + */ + return CalculateFinalPositionVelocity(tsince, e, + a, omega, xl, xnode, + xincl, perturbed_xlcof, perturbed_aycof, + perturbed_x3thm1, perturbed_x1mth2, perturbed_x7thm1, + perturbed_cosio, perturbed_sinio); + +} + +Eci SGP4::FindPositionSGP4(double tsince) const +{ + /* + * the final values + */ + double e; + double a; + double omega; + double xl; + double xnode; + double xincl; + + /* + * update for secular gravity and atmospheric drag + */ + const double xmdf = elements_.MeanAnomoly() + common_consts_.xmdot * tsince; + const double omgadf = elements_.ArgumentPerigee() + common_consts_.omgdot * tsince; + const double xnoddf = elements_.AscendingNode() + common_consts_.xnodot * tsince; + + const double tsq = tsince * tsince; + xnode = xnoddf + common_consts_.xnodcf * tsq; + double tempa = 1.0 - common_consts_.c1 * tsince; + double tempe = elements_.BStar() * common_consts_.c4 * tsince; + double templ = common_consts_.t2cof * tsq; + + xincl = elements_.Inclination(); + omega = omgadf; + double xmp = xmdf; + + if (!use_simple_model_) + { + const double delomg = nearspace_consts_.omgcof * tsince; + const double delm = nearspace_consts_.xmcof * (pow(1.0 + common_consts_.eta * cos(xmdf), 3.0) - nearspace_consts_.delmo); + const double temp = delomg + delm; + + xmp += temp; + omega -= temp; + + const double tcube = tsq * tsince; + const double tfour = tsince * tcube; + + tempa = tempa - nearspace_consts_.d2 * tsq - nearspace_consts_.d3 * tcube - nearspace_consts_.d4 * tfour; + tempe += elements_.BStar() * nearspace_consts_.c5 * (sin(xmp) - nearspace_consts_.sinmo); + templ += nearspace_consts_.t3cof * tcube + tfour * (nearspace_consts_.t4cof + tsince * nearspace_consts_.t5cof); + } + + a = elements_.RecoveredSemiMajorAxis() * pow(tempa, 2.0); + e = elements_.Eccentricity() - tempe; + xl = xmp + omega + xnode + elements_.RecoveredMeanMotion() * templ; + + if (xl <= 0.0) + { + throw SatelliteException("Error: #2 (xl <= 0.0)"); + } + + /* + * fix tolerance for error recognition + */ + if (e >= 1.0 || e < -0.001) + { + throw SatelliteException("Error: #1 (e >= 1.0 || e < -0.001)"); + } + /* + * fix tolerance to avoid a divide by zero + */ + if (e < 1.0e-6) + { + e = 1.0e-6; + } + + /* + * using calculated values, find position and velocity + * we can pass in constants from Initialise() as these dont change + */ + return CalculateFinalPositionVelocity(tsince, e, + a, omega, xl, xnode, + xincl, common_consts_.xlcof, common_consts_.aycof, + common_consts_.x3thm1, common_consts_.x1mth2, common_consts_.x7thm1, + common_consts_.cosio, common_consts_.sinio); + +} + +Eci SGP4::CalculateFinalPositionVelocity(const double& tsince, const double& e, + const double& a, const double& omega, const double& xl, const double& xnode, + const double& xincl, const double& xlcof, const double& aycof, + const double& x3thm1, const double& x1mth2, const double& x7thm1, + const double& cosio, const double& sinio) const +{ + const double beta = sqrt(1.0 - e * e); + const double xn = kXKE / pow(a, 1.5); + /* + * long period periodics + */ + const double axn = e * cos(omega); + const double temp11 = 1.0 / (a * beta * beta); + const double xll = temp11 * xlcof * axn; + const double aynl = temp11 * aycof; + const double xlt = xl + xll; + const double ayn = e * sin(omega) + aynl; + const double elsq = axn * axn + ayn * ayn; + + /* + * solve keplers equation + * - solve using Newton-Raphson root solving + * - here capu is almost the mean anomoly + * - initialise the eccentric anomaly term epw + * - The fmod saves reduction of angle to +/-2pi in sin/cos() and prevents + * convergence problems. + */ + const double capu = fmod(xlt - xnode, kTWOPI); + double epw = capu; + + double sinepw = 0.0; + double cosepw = 0.0; + double ecose = 0.0; + double esine = 0.0; + + /* + * sensibility check for N-R correction + */ + const double max_newton_naphson = 1.25 * fabs(sqrt(elsq)); + + bool kepler_running = true; + + for (int i = 0; i < 10 && kepler_running; i++) + { + sinepw = sin(epw); + cosepw = cos(epw); + ecose = axn * cosepw + ayn * sinepw; + esine = axn * sinepw - ayn * cosepw; + + double f = capu - epw + esine; + + if (fabs(f) < 1.0e-12) + { + kepler_running = false; + } + else + { + /* + * 1st order Newton-Raphson correction + */ + const double fdot = 1.0 - ecose; + double delta_epw = f / fdot; + + /* + * 2nd order Newton-Raphson correction. + * f / (fdot - 0.5 * d2f * f/fdot) + */ + if (i == 0) + { + if (delta_epw > max_newton_naphson) + { + delta_epw = max_newton_naphson; + } + else if (delta_epw < -max_newton_naphson) + { + delta_epw = -max_newton_naphson; + } + } + else + { + delta_epw = f / (fdot + 0.5 * esine * delta_epw); + } + + /* + * Newton-Raphson correction of -F/DF + */ + epw += delta_epw; + } + } + /* + * short period preliminary quantities + */ + const double temp21 = 1.0 - elsq; + const double pl = a * temp21; + + if (pl < 0.0) + { + throw SatelliteException("Error: #4 (pl < 0.0)"); + } + + const double r = a * (1.0 - ecose); + const double temp31 = 1.0 / r; + const double rdot = kXKE * sqrt(a) * esine * temp31; + const double rfdot = kXKE * sqrt(pl) * temp31; + const double temp32 = a * temp31; + const double betal = sqrt(temp21); + const double temp33 = 1.0 / (1.0 + betal); + const double cosu = temp32 * (cosepw - axn + ayn * esine * temp33); + const double sinu = temp32 * (sinepw - ayn - axn * esine * temp33); + const double u = atan2(sinu, cosu); + const double sin2u = 2.0 * sinu * cosu; + const double cos2u = 2.0 * cosu * cosu - 1.0; + + /* + * update for short periodics + */ + const double temp41 = 1.0 / pl; + const double temp42 = kCK2 * temp41; + const double temp43 = temp42 * temp41; + + const double rk = r * (1.0 - 1.5 * temp43 * betal * x3thm1) + 0.5 * temp42 * x1mth2 * cos2u; + const double uk = u - 0.25 * temp43 * x7thm1 * sin2u; + const double xnodek = xnode + 1.5 * temp43 * cosio * sin2u; + const double xinck = xincl + 1.5 * temp43 * cosio * sinio * cos2u; + const double rdotk = rdot - xn * temp42 * x1mth2 * sin2u; + const double rfdotk = rfdot + xn * temp42 * (x1mth2 * cos2u + 1.5 * x3thm1); + + if (rk < 1.0) + { + throw SatelliteException("Error: #6 Satellite decayed (rk < 1.0)"); + } + + /* + * orientation vectors + */ + const double sinuk = sin(uk); + const double cosuk = cos(uk); + const double sinik = sin(xinck); + const double cosik = cos(xinck); + const double sinnok = sin(xnodek); + const double cosnok = cos(xnodek); + const double xmx = -sinnok * cosik; + const double xmy = cosnok * cosik; + const double ux = xmx * sinuk + cosnok * cosuk; + const double uy = xmy * sinuk + sinnok * cosuk; + const double uz = sinik * sinuk; + const double vx = xmx * cosuk - cosnok * sinuk; + const double vy = xmy * cosuk - sinnok * sinuk; + const double vz = sinik * cosuk; + /* + * position and velocity + */ + const double x = rk * ux * kXKMPER; + const double y = rk * uy * kXKMPER; + const double z = rk * uz * kXKMPER; + Vector position(x, y, z); + const double xdot = (rdotk * ux + rfdotk * vx) * kXKMPER / 60.0; + const double ydot = (rdotk * uy + rfdotk * vy) * kXKMPER / 60.0; + const double zdot = (rdotk * uz + rfdotk * vz) * kXKMPER / 60.0; + Vector velocity(xdot, ydot, zdot); + + Julian julian(elements_.Epoch()); + julian.AddMin(tsince); + + Eci eci(julian, position, velocity); + + return eci; +} + +static inline double EvaluateCubicPolynomial(const double x, const double constant, + const double linear, const double squared, const double cubed) +{ + return constant + x * (linear + x * (squared + x * cubed)); +} + +/* + * deep space initialisation + */ +void SGP4::DeepSpaceInitialise(const double& eosq, const double& sinio, const double& cosio, const double& betao, + const double& theta2, const double& betao2, + const double& xmdot, const double& omgdot, const double& xnodot) +{ + double se = 0.0; + double si = 0.0; + double sl = 0.0; + double sgh = 0.0; + double shdq = 0.0; + + double bfact = 0.0; + + static const double ZNS = 1.19459E-5; + static const double C1SS = 2.9864797E-6; + static const double ZES = 0.01675; + static const double ZNL = 1.5835218E-4; + static const double C1L = 4.7968065E-7; + static const double ZEL = 0.05490; + static const double ZCOSIS = 0.91744867; + static const double ZSINI = 0.39785416; + static const double ZSINGS = -0.98088458; + static const double ZCOSGS = 0.1945905; + static const double Q22 = 1.7891679E-6; + static const double Q31 = 2.1460748E-6; + static const double Q33 = 2.2123015E-7; + static const double ROOT22 = 1.7891679E-6; + static const double ROOT32 = 3.7393792E-7; + static const double ROOT44 = 7.3636953E-9; + static const double ROOT52 = 1.1428639E-7; + static const double ROOT54 = 2.1765803E-9; + + const double aqnv = 1.0 / elements_.RecoveredSemiMajorAxis(); + const double xpidot = omgdot + xnodot; + const double sinq = sin(elements_.AscendingNode()); + const double cosq = cos(elements_.AscendingNode()); + const double sing = sin(elements_.ArgumentPerigee()); + const double cosg = cos(elements_.ArgumentPerigee()); + + /* + * initialize lunar / solar terms + */ + const double jday = elements_.Epoch().FromJan1_12h_1900(); + + const double xnodce = 4.5236020 - 9.2422029e-4 * jday; + const double xnodce_temp = fmod(xnodce, kTWOPI); + const double stem = sin(xnodce_temp); + const double ctem = cos(xnodce_temp); + const double zcosil = 0.91375164 - 0.03568096 * ctem; + const double zsinil = sqrt(1.0 - zcosil * zcosil); + const double zsinhl = 0.089683511 * stem / zsinil; + const double zcoshl = sqrt(1.0 - zsinhl * zsinhl); + const double c = 4.7199672 + 0.22997150 * jday; + const double gam = 5.8351514 + 0.0019443680 * jday; + deepspace_consts_.zmol = Util::WrapTwoPI(c - gam); + double zx = 0.39785416 * stem / zsinil; + double zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; + zx = atan2(zx, zy); + zx = fmod(gam + zx - xnodce, kTWOPI); + + const double zcosgl = cos(zx); + const double zsingl = sin(zx); + deepspace_consts_.zmos = Util::WrapTwoPI(6.2565837 + 0.017201977 * jday); + + /* + * do solar terms + */ + double zcosg = ZCOSGS; + double zsing = ZSINGS; + double zcosi = ZCOSIS; + double zsini = ZSINI; + double zcosh = cosq; + double zsinh = sinq; + double cc = C1SS; + double zn = ZNS; + double ze = ZES; + const double xnoi = 1.0 / elements_.RecoveredMeanMotion(); + + for (int cnt = 0; cnt < 2; cnt++) + { + /* + * solar terms are done a second time after lunar terms are done + */ + const double a1 = zcosg * zcosh + zsing * zcosi * zsinh; + const double a3 = -zsing * zcosh + zcosg * zcosi * zsinh; + const double a7 = -zcosg * zsinh + zsing * zcosi * zcosh; + const double a8 = zsing * zsini; + const double a9 = zsing * zsinh + zcosg * zcosi*zcosh; + const double a10 = zcosg * zsini; + const double a2 = cosio * a7 + sinio * a8; + const double a4 = cosio * a9 + sinio * a10; + const double a5 = -sinio * a7 + cosio * a8; + const double a6 = -sinio * a9 + cosio * a10; + const double x1 = a1 * cosg + a2 * sing; + const double x2 = a3 * cosg + a4 * sing; + const double x3 = -a1 * sing + a2 * cosg; + const double x4 = -a3 * sing + a4 * cosg; + const double x5 = a5 * sing; + const double x6 = a6 * sing; + const double x7 = a5 * cosg; + const double x8 = a6 * cosg; + const double z31 = 12.0 * x1 * x1 - 3. * x3 * x3; + const double z32 = 24.0 * x1 * x2 - 6. * x3 * x4; + const double z33 = 12.0 * x2 * x2 - 3. * x4 * x4; + double z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * eosq; + double z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * eosq; + double z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * eosq; + const double z11 = -6.0 * a1 * a5 + eosq * (-24. * x1 * x7 - 6. * x3 * x5); + const double z12 = -6.0 * (a1 * a6 + a3 * a5) + eosq * (-24. * (x2 * x7 + x1 * x8) - 6. * (x3 * x6 + x4 * x5)); + const double z13 = -6.0 * a3 * a6 + eosq * (-24. * x2 * x8 - 6. * x4 * x6); + const double z21 = 6.0 * a2 * a5 + eosq * (24. * x1 * x5 - 6. * x3 * x7); + const double z22 = 6.0 * (a4 * a5 + a2 * a6) + eosq * (24. * (x2 * x5 + x1 * x6) - 6. * (x4 * x7 + x3 * x8)); + const double z23 = 6.0 * a4 * a6 + eosq * (24. * x2 * x6 - 6. * x4 * x8); + z1 = z1 + z1 + betao2 * z31; + z2 = z2 + z2 + betao2 * z32; + z3 = z3 + z3 + betao2 * z33; + const double s3 = cc * xnoi; + const double s2 = -0.5 * s3 / betao; + const double s4 = s3 * betao; + const double s1 = -15.0 * elements_.Eccentricity() * s4; + const double s5 = x1 * x3 + x2 * x4; + const double s6 = x2 * x3 + x1 * x4; + const double s7 = x2 * x4 - x1 * x3; + se = s1 * zn * s5; + si = s2 * zn * (z11 + z13); + sl = -zn * s3 * (z1 + z3 - 14.0 - 6.0 * eosq); + sgh = s4 * zn * (z31 + z33 - 6.0); + + /* + * replaced + * sh = -zn * s2 * (z21 + z23 + * with + * shdq = (-zn * s2 * (z21 + z23)) / sinio + */ + if (elements_.Inclination() < 5.2359877e-2 || elements_.Inclination() > kPI - 5.2359877e-2) + { + shdq = 0.0; + } + else + { + shdq = (-zn * s2 * (z21 + z23)) / sinio; + } + + deepspace_consts_.ee2 = 2.0 * s1 * s6; + deepspace_consts_.e3 = 2.0 * s1 * s7; + deepspace_consts_.xi2 = 2.0 * s2 * z12; + deepspace_consts_.xi3 = 2.0 * s2 * (z13 - z11); + deepspace_consts_.xl2 = -2.0 * s3 * z2; + deepspace_consts_.xl3 = -2.0 * s3 * (z3 - z1); + deepspace_consts_.xl4 = -2.0 * s3 * (-21.0 - 9.0 * eosq) * ze; + deepspace_consts_.xgh2 = 2.0 * s4 * z32; + deepspace_consts_.xgh3 = 2.0 * s4 * (z33 - z31); + deepspace_consts_.xgh4 = -18.0 * s4 * ze; + deepspace_consts_.xh2 = -2.0 * s2 * z22; + deepspace_consts_.xh3 = -2.0 * s2 * (z23 - z21); + + if (cnt == 1) + { + break; + } + /* + * do lunar terms + */ + deepspace_consts_.sse = se; + deepspace_consts_.ssi = si; + deepspace_consts_.ssl = sl; + deepspace_consts_.ssh = shdq; + deepspace_consts_.ssg = sgh - cosio * deepspace_consts_.ssh; + deepspace_consts_.se2 = deepspace_consts_.ee2; + deepspace_consts_.si2 = deepspace_consts_.xi2; + deepspace_consts_.sl2 = deepspace_consts_.xl2; + deepspace_consts_.sgh2 = deepspace_consts_.xgh2; + deepspace_consts_.sh2 = deepspace_consts_.xh2; + deepspace_consts_.se3 = deepspace_consts_.e3; + deepspace_consts_.si3 = deepspace_consts_.xi3; + deepspace_consts_.sl3 = deepspace_consts_.xl3; + deepspace_consts_.sgh3 = deepspace_consts_.xgh3; + deepspace_consts_.sh3 = deepspace_consts_.xh3; + deepspace_consts_.sl4 = deepspace_consts_.xl4; + deepspace_consts_.sgh4 = deepspace_consts_.xgh4; + zcosg = zcosgl; + zsing = zsingl; + zcosi = zcosil; + zsini = zsinil; + zcosh = zcoshl * cosq + zsinhl * sinq; + zsinh = sinq * zcoshl - cosq * zsinhl; + zn = ZNL; + cc = C1L; + ze = ZEL; + } + + deepspace_consts_.sse += se; + deepspace_consts_.ssi += si; + deepspace_consts_.ssl += sl; + deepspace_consts_.ssg += sgh - cosio * shdq; + deepspace_consts_.ssh += shdq; + + deepspace_consts_.resonance_flag = false; + deepspace_consts_.synchronous_flag = false; + bool initialise_integrator = true; + + if (elements_.RecoveredMeanMotion() < 0.0052359877 && elements_.RecoveredMeanMotion() > 0.0034906585) + { + /* + * 24h synchronous resonance terms initialisation + */ + deepspace_consts_.resonance_flag = true; + deepspace_consts_.synchronous_flag = true; + + const double g200 = 1.0 + eosq * (-2.5 + 0.8125 * eosq); + const double g310 = 1.0 + 2.0 * eosq; + const double g300 = 1.0 + eosq * (-6.0 + 6.60937 * eosq); + const double f220 = 0.75 * (1.0 + cosio) * (1.0 + cosio); + const double f311 = 0.9375 * sinio * sinio * (1.0 + 3.0 * cosio) - 0.75 * (1.0 + cosio); + double f330 = 1.0 + cosio; + f330 = 1.875 * f330 * f330 * f330; + deepspace_consts_.del1 = 3.0 * elements_.RecoveredMeanMotion() * elements_.RecoveredMeanMotion() * aqnv * aqnv; + deepspace_consts_.del2 = 2.0 * deepspace_consts_.del1 * f220 * g200 * Q22; + deepspace_consts_.del3 = 3.0 * deepspace_consts_.del1 * f330 * g300 * Q33 * aqnv; + deepspace_consts_.del1 = deepspace_consts_.del1 * f311 * g310 * Q31 * aqnv; + + integrator_consts_.xlamo = elements_.MeanAnomoly() + elements_.AscendingNode() + elements_.ArgumentPerigee() - deepspace_consts_.gsto; + bfact = xmdot + xpidot - kTHDT; + bfact += deepspace_consts_.ssl + deepspace_consts_.ssg + deepspace_consts_.ssh; + + } + else if (elements_.RecoveredMeanMotion() < 8.26e-3 || elements_.RecoveredMeanMotion() > 9.24e-3 || elements_.Eccentricity() < 0.5) + { + initialise_integrator = false; + } + else + { + /* + * geopotential resonance initialisation for 12 hour orbits + */ + deepspace_consts_.resonance_flag = true; + + double g211; + double g310; + double g322; + double g410; + double g422; + double g520; + + double g201 = -0.306 - (elements_.Eccentricity() - 0.64) * 0.440; + + if (elements_.Eccentricity() <= 0.65) + { + g211 = EvaluateCubicPolynomial(elements_.Eccentricity(), 3.616, -13.247, +16.290, 0.0); + g310 = EvaluateCubicPolynomial(elements_.Eccentricity(), -19.302, 117.390, -228.419, 156.591); + g322 = EvaluateCubicPolynomial(elements_.Eccentricity(), -18.9068, 109.7927, -214.6334, 146.5816); + g410 = EvaluateCubicPolynomial(elements_.Eccentricity(), -41.122, 242.694, -471.094, 313.953); + g422 = EvaluateCubicPolynomial(elements_.Eccentricity(), -146.407, 841.880, -1629.014, 1083.435); + g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), -532.114, 3017.977, -5740.032, 3708.276); + } + else + { + g211 = EvaluateCubicPolynomial(elements_.Eccentricity(), -72.099, 331.819, -508.738, 266.724); + g310 = EvaluateCubicPolynomial(elements_.Eccentricity(), -346.844, 1582.851, -2415.925, 1246.113); + g322 = EvaluateCubicPolynomial(elements_.Eccentricity(), -342.585, 1554.908, -2366.899, 1215.972); + g410 = EvaluateCubicPolynomial(elements_.Eccentricity(), -1052.797, 4758.686, -7193.992, 3651.957); + g422 = EvaluateCubicPolynomial(elements_.Eccentricity(), -3581.69, 16178.11, -24462.77, 12422.52); + + if (elements_.Eccentricity() <= 0.715) + { + g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), 1464.74, -4664.75, 3763.64, 0.0); + } + else + { + g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), -5149.66, 29936.92, -54087.36, 31324.56); + } + } + + double g533; + double g521; + double g532; + + if (elements_.Eccentricity() < 0.7) + { + g533 = EvaluateCubicPolynomial(elements_.Eccentricity(), -919.2277, 4988.61, -9064.77, 5542.21); + g521 = EvaluateCubicPolynomial(elements_.Eccentricity(), -822.71072, 4568.6173, -8491.4146, 5337.524); + g532 = EvaluateCubicPolynomial(elements_.Eccentricity(), -853.666, 4690.25, -8624.77, 5341.4); + } + else + { + g533 = EvaluateCubicPolynomial(elements_.Eccentricity(), -37995.78, 161616.52, -229838.2, 109377.94); + g521 = EvaluateCubicPolynomial(elements_.Eccentricity(), -51752.104, 218913.95, -309468.16, 146349.42); + g532 = EvaluateCubicPolynomial(elements_.Eccentricity(), -40023.88, 170470.89, -242699.48, 115605.82); + } + + const double sini2 = sinio * sinio; + const double f220 = 0.75 * (1.0 + 2.0 * cosio + theta2); + const double f221 = 1.5 * sini2; + const double f321 = 1.875 * sinio * (1.0 - 2.0 * cosio - 3.0 * theta2); + const double f322 = -1.875 * sinio * (1.0 + 2.0 * cosio - 3.0 * theta2); + const double f441 = 35.0 * sini2 * f220; + const double f442 = 39.3750 * sini2 * sini2; + const double f522 = 9.84375 * sinio * (sini2 * (1.0 - 2.0 * cosio - 5.0 * theta2) + + 0.33333333 * (-2.0 + 4.0 * cosio + 6.0 * theta2)); + const double f523 = sinio * (4.92187512 * sini2 * (-2.0 - 4.0 * cosio + 10.0 * theta2) + + 6.56250012 * (1.0 + 2.0 * cosio - 3.0 * theta2)); + const double f542 = 29.53125 * sinio * (2.0 - 8.0 * cosio + theta2 * + (-12.0 + 8.0 * cosio + 10.0 * theta2)); + const double f543 = 29.53125 * sinio * (-2.0 - 8.0 * cosio + theta2 * + (12.0 + 8.0 * cosio - 10.0 * theta2)); + + const double xno2 = elements_.RecoveredMeanMotion() * elements_.RecoveredMeanMotion(); + const double ainv2 = aqnv * aqnv; + + double temp1 = 3.0 * xno2 * ainv2; + double temp = temp1 * ROOT22; + deepspace_consts_.d2201 = temp * f220 * g201; + deepspace_consts_.d2211 = temp * f221 * g211; + temp1 = temp1 * aqnv; + temp = temp1 * ROOT32; + deepspace_consts_.d3210 = temp * f321 * g310; + deepspace_consts_.d3222 = temp * f322 * g322; + temp1 = temp1 * aqnv; + temp = 2.0 * temp1 * ROOT44; + deepspace_consts_.d4410 = temp * f441 * g410; + deepspace_consts_.d4422 = temp * f442 * g422; + temp1 = temp1 * aqnv; + temp = temp1 * ROOT52; + deepspace_consts_.d5220 = temp * f522 * g520; + deepspace_consts_.d5232 = temp * f523 * g532; + temp = 2.0 * temp1 * ROOT54; + deepspace_consts_.d5421 = temp * f542 * g521; + deepspace_consts_.d5433 = temp * f543 * g533; + + integrator_consts_.xlamo = elements_.MeanAnomoly() + elements_.AscendingNode() + elements_.AscendingNode() - deepspace_consts_.gsto - deepspace_consts_.gsto; + bfact = xmdot + xnodot + xnodot - kTHDT - kTHDT; + bfact = bfact + deepspace_consts_.ssl + deepspace_consts_.ssh + deepspace_consts_.ssh; + } + + if (initialise_integrator) + { + /* + * initialise integrator + */ + integrator_consts_.xfact = bfact - elements_.RecoveredMeanMotion(); + integrator_params_.atime = 0.0; + integrator_params_.xni = elements_.RecoveredMeanMotion(); + integrator_params_.xli = integrator_consts_.xlamo; + /* + * precompute dot terms for epoch + */ + DeepSpaceCalcDotTerms(&integrator_consts_.values_0); + } +} + +void SGP4::DeepSpaceCalculateLunarSolarTerms(const double t, double* pe, double* pinc, + double* pl, double* pgh, double* ph) const +{ + static const double ZES = 0.01675; + static const double ZNS = 1.19459E-5; + static const double ZNL = 1.5835218E-4; + static const double ZEL = 0.05490; + + /* + * calculate solar terms for time t + */ + double zm = deepspace_consts_.zmos + ZNS * t; + double zf = zm + 2.0 * ZES * sin(zm); + double sinzf = sin(zf); + double f2 = 0.5 * sinzf * sinzf - 0.25; + double f3 = -0.5 * sinzf * cos(zf); + const double ses = deepspace_consts_.se2 * f2 + deepspace_consts_.se3 * f3; + const double sis = deepspace_consts_.si2 * f2 + deepspace_consts_.si3 * f3; + const double sls = deepspace_consts_.sl2 * f2 + deepspace_consts_.sl3 * f3 + deepspace_consts_.sl4 * sinzf; + const double sghs = deepspace_consts_.sgh2 * f2 + deepspace_consts_.sgh3 * f3 + deepspace_consts_.sgh4 * sinzf; + const double shs = deepspace_consts_.sh2 * f2 + deepspace_consts_.sh3 * f3; + + /* + * calculate lunar terms for time t + */ + zm = deepspace_consts_.zmol + ZNL * t; + zf = zm + 2.0 * ZEL * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + const double sel = deepspace_consts_.ee2 * f2 + deepspace_consts_.e3 * f3; + const double sil = deepspace_consts_.xi2 * f2 + deepspace_consts_.xi3 * f3; + const double sll = deepspace_consts_.xl2 * f2 + deepspace_consts_.xl3 * f3 + deepspace_consts_.xl4 * sinzf; + const double sghl = deepspace_consts_.xgh2 * f2 + deepspace_consts_.xgh3 * f3 + deepspace_consts_.xgh4 * sinzf; + const double shl = deepspace_consts_.xh2 * f2 + deepspace_consts_.xh3 * f3; + + /* + * merge calculated values + */ + (*pe) = ses + sel; + (*pinc) = sis + sil; + (*pl) = sls + sll; + (*pgh) = sghs + sghl; + (*ph) = shs + shl; +} + +/* + * calculate lunar / solar periodics and apply + */ +void SGP4::DeepSpacePeriodics(const double& t, double* em, + double* xinc, double* omgasm, double* xnodes, double* xll) const +{ + /* + * storage for lunar / solar terms set by DeepSpaceCalculateLunarSolarTerms() + */ + double pe = 0.0; + double pinc = 0.0; + double pl = 0.0; + double pgh = 0.0; + double ph = 0.0; + + /* + * calculate lunar / solar terms for current time + */ + DeepSpaceCalculateLunarSolarTerms(t, &pe, &pinc, &pl, &pgh, &ph); + + (*xinc) += pinc; + (*em) += pe; + + /* Spacetrack report #3 has sin/cos from before perturbations + * added to xinc (oldxinc), but apparently report # 6 has then + * from after they are added. + * use for strn3 + * if (elements_.Inclination() >= 0.2) + * use for gsfc + * if (xinc >= 0.2) + * (moved from start of function) + */ + const double sinis = sin(*xinc); + const double cosis = cos(*xinc); + + if ((*xinc) >= 0.2) + { + /* + * apply periodics directly + */ + const double tmp_ph = ph / sinis; + + (*omgasm) += pgh - cosis * tmp_ph; + (*xnodes) += tmp_ph; + (*xll) += pl; + } + else + { + /* + * apply periodics with lyddane modification + */ + const double sinok = sin(*xnodes); + const double cosok = cos(*xnodes); + double alfdp = sinis * sinok; + double betdp = sinis * cosok; + const double dalf = ph * cosok + pinc * cosis * sinok; + const double dbet = -ph * sinok + pinc * cosis * cosok; + + alfdp += dalf; + betdp += dbet; + + (*xnodes) = Util::WrapTwoPI((*xnodes)); + + double xls = (*xll) + (*omgasm) + cosis * (*xnodes); + double dls = pl + pgh - pinc * (*xnodes) * sinis; + xls += dls; + + /* + * save old xnodes value + */ + const double oldxnodes = (*xnodes); + + (*xnodes) = atan2(alfdp, betdp); + if ((*xnodes) < 0.0) + { + (*xnodes) += kTWOPI; + } + + /* + * Get perturbed xnodes in to same quadrant as original. + * RAAN is in the range of 0 to 360 degrees + * atan2 is in the range of -180 to 180 degrees + */ + if (fabs(oldxnodes - (*xnodes)) > kPI) + { + if ((*xnodes) < oldxnodes) + { + (*xnodes) += kTWOPI; + } + else + { + (*xnodes) -= kTWOPI; + } + } + + (*xll) += pl; + (*omgasm) = xls - (*xll) - cosis * (*xnodes); + } +} + +/* + * deep space secular effects + */ +void SGP4::DeepSpaceSecular(const double& t, double* xll, double* omgasm, + double* xnodes, double* em, double* xinc, double* xn) const +{ + static const double STEP = 720.0; + static const double STEP2 = 259200.0; + + (*xll) += deepspace_consts_.ssl * t; + (*omgasm) += deepspace_consts_.ssg * t; + (*xnodes) += deepspace_consts_.ssh * t; + (*em) += deepspace_consts_.sse * t; + (*xinc) += deepspace_consts_.ssi * t; + + if (!deepspace_consts_.resonance_flag) + { + return; + } + + /* + * 1st condition (if t is less than one time step from epoch) + * 2nd condition (if integrator_params_.atime and t are of opposite signs, so zero crossing required) + * 3rd condition (if t is closer to zero than integrator_params_.atime, only integrate away from zero) + */ + if (fabs(t) < STEP || + t * integrator_params_.atime <= 0.0 || + fabs(t) < fabs(integrator_params_.atime)) + { + /* + * restart from epoch + */ + integrator_params_.atime = 0.0; + integrator_params_.xni = elements_.RecoveredMeanMotion(); + integrator_params_.xli = integrator_consts_.xlamo; + + /* + * restore precomputed values for epoch + */ + integrator_params_.values_t = integrator_consts_.values_0; + } + + double ft = t - integrator_params_.atime; + + /* + * if time difference (ft) is greater than the time step (720.0) + * loop around until integrator_params_.atime is within one time step of t + */ + if (fabs(ft) >= STEP) + { + /* + * calculate step direction to allow integrator_params_.atime + * to catch up with t + */ + double delt = -STEP; + if (ft >= 0.0) + { + delt = STEP; + } + + do + { + /* + * integrate using current dot terms + */ + DeepSpaceIntegrator(delt, STEP2, integrator_params_.values_t); + + /* + * calculate dot terms for next integration + */ + DeepSpaceCalcDotTerms(&integrator_params_.values_t); + + ft = t - integrator_params_.atime; + } while (fabs(ft) >= STEP); + } + + /* + * integrator + */ + (*xn) = integrator_params_.xni + integrator_params_.values_t.xndot * ft + integrator_params_.values_t.xnddt * ft * ft * 0.5; + const double xl = integrator_params_.xli + integrator_params_.values_t.xldot * ft + integrator_params_.values_t.xndot * ft * ft * 0.5; + const double temp = -(*xnodes) + deepspace_consts_.gsto + t * kTHDT; + + if (deepspace_consts_.synchronous_flag) + { + (*xll) = xl + temp - (*omgasm); + } + else + { + (*xll) = xl + temp + temp; + } +} + +/* + * calculate dot terms + */ +void SGP4::DeepSpaceCalcDotTerms(struct IntegratorValues *values) const +{ + static const double G22 = 5.7686396; + static const double G32 = 0.95240898; + static const double G44 = 1.8014998; + static const double G52 = 1.0508330; + static const double G54 = 4.4108898; + static const double FASX2 = 0.13130908; + static const double FASX4 = 2.8843198; + static const double FASX6 = 0.37448087; + + if (deepspace_consts_.synchronous_flag) + { + + values->xndot = deepspace_consts_.del1 * sin(integrator_params_.xli - FASX2) + + deepspace_consts_.del2 * sin(2.0 * (integrator_params_.xli - FASX4)) + + deepspace_consts_.del3 * sin(3.0 * (integrator_params_.xli - FASX6)); + values->xnddt = deepspace_consts_.del1 * cos(integrator_params_.xli - FASX2) + 2.0 * + deepspace_consts_.del2 * cos(2.0 * (integrator_params_.xli - FASX4)) + 3.0 * + deepspace_consts_.del3 * cos(3.0 * (integrator_params_.xli - FASX6)); + + } + else + { + const double xomi = elements_.ArgumentPerigee() + common_consts_.omgdot * integrator_params_.atime; + const double x2omi = xomi + xomi; + const double x2li = integrator_params_.xli + integrator_params_.xli; + + values->xndot = deepspace_consts_.d2201 * sin(x2omi + integrator_params_.xli - G22) + + deepspace_consts_.d2211 * sin(integrator_params_.xli - G22) + + deepspace_consts_.d3210 * sin(xomi + integrator_params_.xli - G32) + + deepspace_consts_.d3222 * sin(-xomi + integrator_params_.xli - G32) + + deepspace_consts_.d4410 * sin(x2omi + x2li - G44) + + deepspace_consts_.d4422 * sin(x2li - G44) + + deepspace_consts_.d5220 * sin(xomi + integrator_params_.xli - G52) + + deepspace_consts_.d5232 * sin(-xomi + integrator_params_.xli - G52) + + deepspace_consts_.d5421 * sin(xomi + x2li - G54) + + deepspace_consts_.d5433 * sin(-xomi + x2li - G54); + values->xnddt = deepspace_consts_.d2201 * cos(x2omi + integrator_params_.xli - G22) + + deepspace_consts_.d2211 * cos(integrator_params_.xli - G22) + + deepspace_consts_.d3210 * cos(xomi + integrator_params_.xli - G32) + + deepspace_consts_.d3222 * cos(-xomi + integrator_params_.xli - G32) + + deepspace_consts_.d5220 * cos(xomi + integrator_params_.xli - G52) + + deepspace_consts_.d5232 * cos(-xomi + integrator_params_.xli - G52) + + 2.0 * (deepspace_consts_.d4410 * cos(x2omi + x2li - G44) + + deepspace_consts_.d4422 * cos(x2li - G44) + + deepspace_consts_.d5421 * cos(xomi + x2li - G54) + + deepspace_consts_.d5433 * cos(-xomi + x2li - G54)); + } + + values->xldot = integrator_params_.xni + integrator_consts_.xfact; + values->xnddt *= values->xldot; +} + +/* + * deep space integrator for time period of delt + */ +void SGP4::DeepSpaceIntegrator(const double delt, const double step2, + const struct IntegratorValues &values) const +{ + /* + * integrator + */ + integrator_params_.xli += values.xldot * delt + values.xndot * step2; + integrator_params_.xni += values.xndot * delt + values.xnddt * step2; + + /* + * increment integrator time + */ + integrator_params_.atime += delt; +} + +void SGP4::Reset() +{ + /* + * common variables + */ + use_simple_model_ = false; + use_deep_space_ = false; +} diff --git a/SolarPosition.cpp b/SolarPosition.cpp index cfa5e78..d5bf218 100644 --- a/SolarPosition.cpp +++ b/SolarPosition.cpp @@ -1,46 +1,46 @@ -#include "SolarPosition.h" - -#include "Globals.h" -#include "Util.h" - -#include - -Eci SolarPosition::FindPosition(const Julian& j) -{ - const double mjd = j.FromJan1_12h_1900(); - const double year = 1900 + mjd / 365.25; - const double T = (mjd + Delta_ET(year) / kSECONDS_PER_DAY) / 36525.0; - const double M = Util::DegreesToRadians(Util::Wrap360(358.47583 - + Util::Wrap360(35999.04975 * T) - - (0.000150 + 0.0000033 * T) * T * T)); - const double L = Util::DegreesToRadians(Util::Wrap360(279.69668 - + Util::Wrap360(36000.76892 * T) - + 0.0003025 * T*T)); - const double e = 0.01675104 - (0.0000418 + 0.000000126 * T) * T; - const double C = Util::DegreesToRadians((1.919460 - - (0.004789 + 0.000014 * T) * T) * sin(M) - + (0.020094 - 0.000100 * T) * sin(2 * M) - + 0.000293 * sin(3 * M)); - const double O = Util::DegreesToRadians( - Util::Wrap360(259.18 - 1934.142 * T)); - const double Lsa = Util::WrapTwoPI(L + C - - Util::DegreesToRadians(0.00569 - 0.00479 * sin(O))); - const double nu = Util::WrapTwoPI(M + C); - double R = 1.0000002 * (1 - e * e) / (1 + e * cos(nu)); - const double eps = Util::DegreesToRadians(23.452294 - (0.0130125 - + (0.00000164 - 0.000000503 * T) * T) * T + 0.00256 * cos(O)); - R = R * kAU; - - Vector solar_position = Vector(R * cos(Lsa), - R * sin(Lsa) * cos(eps), - R * sin(Lsa) * sin(eps), - R); - - return Eci(j, solar_position); -} - -double SolarPosition::Delta_ET(double year) const -{ - return 26.465 + 0.747622 * (year - 1950) + 1.886913 - * sin(kTWOPI * (year - 1975) / 33); -} +#include "SolarPosition.h" + +#include "Globals.h" +#include "Util.h" + +#include + +Eci SolarPosition::FindPosition(const Julian& j) +{ + const double mjd = j.FromJan1_12h_1900(); + const double year = 1900 + mjd / 365.25; + const double T = (mjd + Delta_ET(year) / kSECONDS_PER_DAY) / 36525.0; + const double M = Util::DegreesToRadians(Util::Wrap360(358.47583 + + Util::Wrap360(35999.04975 * T) + - (0.000150 + 0.0000033 * T) * T * T)); + const double L = Util::DegreesToRadians(Util::Wrap360(279.69668 + + Util::Wrap360(36000.76892 * T) + + 0.0003025 * T*T)); + const double e = 0.01675104 - (0.0000418 + 0.000000126 * T) * T; + const double C = Util::DegreesToRadians((1.919460 + - (0.004789 + 0.000014 * T) * T) * sin(M) + + (0.020094 - 0.000100 * T) * sin(2 * M) + + 0.000293 * sin(3 * M)); + const double O = Util::DegreesToRadians( + Util::Wrap360(259.18 - 1934.142 * T)); + const double Lsa = Util::WrapTwoPI(L + C + - Util::DegreesToRadians(0.00569 - 0.00479 * sin(O))); + const double nu = Util::WrapTwoPI(M + C); + double R = 1.0000002 * (1 - e * e) / (1 + e * cos(nu)); + const double eps = Util::DegreesToRadians(23.452294 - (0.0130125 + + (0.00000164 - 0.000000503 * T) * T) * T + 0.00256 * cos(O)); + R = R * kAU; + + Vector solar_position = Vector(R * cos(Lsa), + R * sin(Lsa) * cos(eps), + R * sin(Lsa) * sin(eps), + R); + + return Eci(j, solar_position); +} + +double SolarPosition::Delta_ET(double year) const +{ + return 26.465 + 0.747622 * (year - 1950) + 1.886913 + * sin(kTWOPI * (year - 1975) / 33); +} diff --git a/Timespan.cpp b/Timespan.cpp index 4440475..9ea7d44 100644 --- a/Timespan.cpp +++ b/Timespan.cpp @@ -1,175 +1,175 @@ -#include "Timespan.h" - -#include "Globals.h" - -Timespan::Timespan() -: time_span_(0.0) { -} - -Timespan::Timespan(const unsigned int days, const unsigned int hours, - const unsigned int minutes, const double seconds) { - - SetValue(days, hours, minutes, seconds); -} - -Timespan::Timespan(const double b) { - - time_span_ = b; -} - -Timespan::Timespan(const Timespan& b) { - time_span_ = b.time_span_; -} - -Timespan::~Timespan(void) { -} - -void Timespan::SetValue(const unsigned int days, const unsigned int hours, - const unsigned int minutes, const double seconds) { - - time_span_ = static_cast (days); - AddHours(hours); - AddMinutes(minutes); - AddSeconds(seconds); -} - -void Timespan::AddDays(const unsigned int days) { - time_span_ += static_cast (days); -} - -void Timespan::AddHours(const unsigned int hours) { - time_span_ += (static_cast (hours) / kHOURS_PER_DAY); -} - -void Timespan::AddMinutes(const unsigned int minutes) { - time_span_ += (static_cast (minutes) / kMINUTES_PER_DAY); -} - -void Timespan::AddSeconds(const double seconds) { - time_span_ += (seconds / kSECONDS_PER_DAY); -} - -double Timespan::GetTotalDays() const { - return time_span_; -} - -double Timespan::GetTotalHours() const { - return time_span_ * kHOURS_PER_DAY; -} - -double Timespan::GetTotalMinutes() const { - return time_span_ * kMINUTES_PER_DAY; -} - -double Timespan::GetTotalSeconds() const { - return time_span_ * kSECONDS_PER_DAY; -} - -Timespan& Timespan::operator =(const Timespan& b) { - - if (this != &b) { - time_span_ = b.time_span_; - } - return (*this); -} - -Timespan Timespan::operator +(const Timespan& b) const { - - return Timespan(*this) += b; -} - -Timespan Timespan::operator -(const Timespan& b) const { - - return Timespan(*this) -= b; -} - -Timespan Timespan::operator/(const double b) const { - - return Timespan(*this) /= b; -} - -Timespan Timespan::operator*(const double b) const { - - return Timespan(*this) *= b; -} - -Timespan & Timespan::operator+=(const Timespan& b) { - - time_span_ += b.time_span_; - return (*this); -} - -Timespan & Timespan::operator-=(const Timespan& b) { - - time_span_ -= b.time_span_; - return (*this); -} - -Timespan & Timespan::operator/=(const double b) { - - time_span_ /= b; - return (*this); -} - -Timespan & Timespan::operator*=(const double b) { - - time_span_ *= b; - return (*this); -} - -bool Timespan::operator ==(const Timespan& b) const { - - if (time_span_ == b.time_span_) - return true; - else - return false; -} - -bool Timespan::operator !=(const Timespan& b) const { - - return !(*this == b); -} - -bool Timespan::operator>(const Timespan& b) const { - - if (time_span_ > b.time_span_) - return true; - else - return false; -} - -bool Timespan::operator<(const Timespan& b) const { - - if (time_span_ < b.time_span_) - return true; - else - return false; -} - -bool Timespan::operator >=(const Timespan& b) const { - - if (time_span_ >= b.time_span_) - return true; - else - return false; -} - -bool Timespan::operator <=(const Timespan & b) const { - - if (time_span_ <= b.time_span_) - return true; - else - return false; -} - -double& operator +=(double& a, const Timespan& b) { - - a += b.time_span_; - return a; -} - -double& operator -=(double& a, const Timespan& b) { - - a -= b.time_span_; - return a; +#include "Timespan.h" + +#include "Globals.h" + +Timespan::Timespan() +: time_span_(0.0) { +} + +Timespan::Timespan(const unsigned int days, const unsigned int hours, + const unsigned int minutes, const double seconds) { + + SetValue(days, hours, minutes, seconds); +} + +Timespan::Timespan(const double b) { + + time_span_ = b; +} + +Timespan::Timespan(const Timespan& b) { + time_span_ = b.time_span_; +} + +Timespan::~Timespan(void) { +} + +void Timespan::SetValue(const unsigned int days, const unsigned int hours, + const unsigned int minutes, const double seconds) { + + time_span_ = static_cast (days); + AddHours(hours); + AddMinutes(minutes); + AddSeconds(seconds); +} + +void Timespan::AddDays(const unsigned int days) { + time_span_ += static_cast (days); +} + +void Timespan::AddHours(const unsigned int hours) { + time_span_ += (static_cast (hours) / kHOURS_PER_DAY); +} + +void Timespan::AddMinutes(const unsigned int minutes) { + time_span_ += (static_cast (minutes) / kMINUTES_PER_DAY); +} + +void Timespan::AddSeconds(const double seconds) { + time_span_ += (seconds / kSECONDS_PER_DAY); +} + +double Timespan::GetTotalDays() const { + return time_span_; +} + +double Timespan::GetTotalHours() const { + return time_span_ * kHOURS_PER_DAY; +} + +double Timespan::GetTotalMinutes() const { + return time_span_ * kMINUTES_PER_DAY; +} + +double Timespan::GetTotalSeconds() const { + return time_span_ * kSECONDS_PER_DAY; +} + +Timespan& Timespan::operator =(const Timespan& b) { + + if (this != &b) { + time_span_ = b.time_span_; + } + return (*this); +} + +Timespan Timespan::operator +(const Timespan& b) const { + + return Timespan(*this) += b; +} + +Timespan Timespan::operator -(const Timespan& b) const { + + return Timespan(*this) -= b; +} + +Timespan Timespan::operator/(const double b) const { + + return Timespan(*this) /= b; +} + +Timespan Timespan::operator*(const double b) const { + + return Timespan(*this) *= b; +} + +Timespan & Timespan::operator+=(const Timespan& b) { + + time_span_ += b.time_span_; + return (*this); +} + +Timespan & Timespan::operator-=(const Timespan& b) { + + time_span_ -= b.time_span_; + return (*this); +} + +Timespan & Timespan::operator/=(const double b) { + + time_span_ /= b; + return (*this); +} + +Timespan & Timespan::operator*=(const double b) { + + time_span_ *= b; + return (*this); +} + +bool Timespan::operator ==(const Timespan& b) const { + + if (time_span_ == b.time_span_) + return true; + else + return false; +} + +bool Timespan::operator !=(const Timespan& b) const { + + return !(*this == b); +} + +bool Timespan::operator>(const Timespan& b) const { + + if (time_span_ > b.time_span_) + return true; + else + return false; +} + +bool Timespan::operator<(const Timespan& b) const { + + if (time_span_ < b.time_span_) + return true; + else + return false; +} + +bool Timespan::operator >=(const Timespan& b) const { + + if (time_span_ >= b.time_span_) + return true; + else + return false; +} + +bool Timespan::operator <=(const Timespan & b) const { + + if (time_span_ <= b.time_span_) + return true; + else + return false; +} + +double& operator +=(double& a, const Timespan& b) { + + a += b.time_span_; + return a; +} + +double& operator -=(double& a, const Timespan& b) { + + a -= b.time_span_; + return a; } \ No newline at end of file