diff --git a/Eci.cpp b/Eci.cpp index 6054a26..f45c455 100644 --- a/Eci.cpp +++ b/Eci.cpp @@ -54,7 +54,7 @@ CoordGeodetic Eci::ToGeodetic() const // 0 >= lon < 360 // const double lon = Fmod2p(theta - date_.ToGreenwichSiderealTime()); // 180 >= lon < 180 - const double lon = fmod(theta - date_.ToGreenwichSiderealTime(), kPI); + const double lon = Util::WrapNegPosPI(theta - date_.ToGreenwichSiderealTime()); const double r = sqrt((position_.x * position_.x) + (position_.y * position_.y)); diff --git a/Julian.cpp b/Julian.cpp index 8c278b8..5b204e7 100644 --- a/Julian.cpp +++ b/Julian.cpp @@ -1,5 +1,6 @@ #include "Globals.h" #include "Julian.h" +#include "Util.h" #include #include @@ -226,7 +227,6 @@ double Julian::ToGreenwichSiderealTime() const return (TWOPI * (GMST / SEC_PER_DAY)); #endif -#if 0 // tut1 = Julian centuries from 2000 Jan. 1 12h UT1 // (since J2000 which is 2451545.0) // a Julian century is 36525 days @@ -237,19 +237,11 @@ double Julian::ToGreenwichSiderealTime() const + 0.093104 * pow(tut1, 2.0) - 0.0000062 * pow(tut1, 3.0); // 360.0 / 86400.0 = 1.0 / 240.0 - theta = fmod(DegreesToRadians(theta / 240.0), kTWOPI); - - /* - * check quadrants - */ - if (theta < 0.0) - { - theta += kTWOPI; - } + theta = Util::WrapTwoPI(Util::DegreesToRadians(theta / 240.0)); return theta; -#endif +#if 0 static const double C1 = 1.72027916940703639e-2; static const double THGR70 = 1.7321343856509374; static const double FK5R = 5.07551419432269442e-15; @@ -264,14 +256,11 @@ double Julian::ToGreenwichSiderealTime() const * find greenwich location at epoch */ const double c1p2p = C1 + kTWOPI; - double gsto = fmod(THGR70 + C1 * ds70 + c1p2p * tfrac - + ts70 * ts70 * FK5R, kTWOPI); - if (gsto < 0.0) - { - gsto += kTWOPI; - } + double gsto = Util::WrapTwoPI(THGR70 + C1 * ds70 + c1p2p * tfrac + + ts70 * ts70 * FK5R); return gsto; +#endif } /* diff --git a/SGP4.cpp b/SGP4.cpp index c97353a..2f9b900 100644 --- a/SGP4.cpp +++ b/SGP4.cpp @@ -627,7 +627,7 @@ void SGP4::DeepSpaceInitialise(const double& eosq, const double& sinio, const do 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::Fmod2p(c - gam); + 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); @@ -635,7 +635,7 @@ void SGP4::DeepSpaceInitialise(const double& eosq, const double& sinio, const do const double zcosgl = cos(zx); const double zsingl = sin(zx); - deepspace_consts_.zmos = Util::Fmod2p(6.2565837 + 0.017201977 * jday); + deepspace_consts_.zmos = Util::WrapTwoPI(6.2565837 + 0.017201977 * jday); /* * do solar terms @@ -1030,11 +1030,7 @@ void SGP4::DeepSpacePeriodics(const double& t, double* em, alfdp += dalf; betdp += dbet; - (*xnodes) = fmod((*xnodes), kTWOPI); - if ((*xnodes) < 0.0) - { - (*xnodes) += kTWOPI; - } + (*xnodes) = Util::WrapTwoPI((*xnodes)); double xls = (*xll) + (*omgasm) + cosis * (*xnodes); double dls = pl + pgh - pinc * (*xnodes) * sinis; diff --git a/Util.h b/Util.h index a47f6d8..9fdab6b 100644 --- a/Util.h +++ b/Util.h @@ -15,16 +15,38 @@ namespace Util return !(ss >> val).fail(); } - - inline double Fmod2p(const double arg) + /* + * always positive result + * Mod(-3,4)= 1 fmod(-3,4)= -3 + */ + inline double Mod(const double x, const double y) { - double modu = fmod(arg, kTWOPI); - if (modu < 0.0) + if (y == 0) { - modu += kTWOPI; + return x; } - return modu; + return x - y * floor(x / y); + } + + inline double WrapNegPosPI(const double a) + { + return Mod(a + kPI, kTWOPI) - kPI; + } + + inline double WrapTwoPI(const double a) + { + return Mod(a, kTWOPI); + } + + inline double WrapNegPos180(const double a) + { + return Mod(a + 180.0, 360.0) - 180.0; + } + + inline double Wrap360(const double a) + { + return Mod(a, 360.0); } inline double DegreesToRadians(const double degrees)