From 19a5e0de308db36c777745d3142db68d0a7d884d Mon Sep 17 00:00:00 2001 From: Jared Dulmage Date: Tue, 3 Sep 2019 10:27:13 -0600 Subject: [PATCH] mobility: (merges !101) Add CartesianToGeographic conversion with test There are cases where it is useful to return to geographic coordinates from cartesian. One example is when you have a mobile node, the position is updated in cartesian coordinates but you may have a need to understand the geographic position or heading when determining position relative to other objects placed geographically, or to compute compass heading or surface speed. --- src/mobility/model/geographic-positions.cc | 104 ++++++++++-- src/mobility/model/geographic-positions.h | 21 +++ src/mobility/test/geo-to-cartesian-test.cc | 176 +++++++++++++++++++-- 3 files changed, 273 insertions(+), 28 deletions(-) diff --git a/src/mobility/model/geographic-positions.cc b/src/mobility/model/geographic-positions.cc index 4e158aa9f..b15716492 100644 --- a/src/mobility/model/geographic-positions.cc +++ b/src/mobility/model/geographic-positions.cc @@ -27,7 +27,7 @@ NS_LOG_COMPONENT_DEFINE ("GeographicPositions"); namespace ns3 { /// Earth's radius in meters if modeled as a perfect sphere -static const double EARTH_RADIUS = 6371e3; +static constexpr double EARTH_RADIUS = 6371e3; /** * GRS80 and WGS84 sources @@ -41,13 +41,19 @@ static const double EARTH_RADIUS = 6371e3; */ /// Earth's semi-major axis in meters as defined by both GRS80 and WGS84 -static const double EARTH_SEMIMAJOR_AXIS = 6378137; +static constexpr double EARTH_SEMIMAJOR_AXIS = 6378137; /// Earth's first eccentricity as defined by GRS80 -static const double EARTH_GRS80_ECCENTRICITY = 0.0818191910428158; +static constexpr double EARTH_GRS80_ECCENTRICITY = 0.0818191910428158; /// Earth's first eccentricity as defined by WGS84 -static const double EARTH_WGS84_ECCENTRICITY = 0.0818191908426215; +static constexpr double EARTH_WGS84_ECCENTRICITY = 0.0818191908426215; + +/// Conversion factor: degrees to radians +static constexpr double DEG2RAD = M_PI / 180.0; + +/// Conversion factor: radians to degrees +static constexpr double RAD2DEG = 180.0 * M_1_PI; Vector GeographicPositions::GeographicToCartesianCoordinates (double latitude, @@ -56,8 +62,8 @@ GeographicPositions::GeographicToCartesianCoordinates (double latitude, EarthSpheroidType sphType) { NS_LOG_FUNCTION_NOARGS (); - double latitudeRadians = 0.01745329 * latitude; - double longitudeRadians = 0.01745329 * longitude; + double latitudeRadians = DEG2RAD * latitude; + double longitudeRadians = DEG2RAD * longitude; double a; // semi-major axis of earth double e; // first eccentricity of earth if (sphType == SPHERE) @@ -85,6 +91,74 @@ GeographicPositions::GeographicToCartesianCoordinates (double latitude, return cartesianCoordinates; } +Vector +GeographicPositions::CartesianToGeographicCoordinates (Vector pos, EarthSpheroidType sphType) +{ + NS_LOG_FUNCTION (pos << sphType); + + double a; // semi-major axis of earth + double e; // first eccentricity of earth + if (sphType == SPHERE) + { + a = EARTH_RADIUS; + e = 0; + } + else if (sphType == GRS80) + { + a = EARTH_SEMIMAJOR_AXIS; + e = EARTH_GRS80_ECCENTRICITY; + } + else // if sphType == WGS84 + { + a = EARTH_SEMIMAJOR_AXIS; + e = EARTH_WGS84_ECCENTRICITY; + } + + Vector lla, tmp; + lla.y = atan2 (pos.y, pos.x); // longitude (rad), in +/- pi + + double e2 = e * e; + // sqrt (pos.x^2 + pos.y^2) + double p = CalculateDistance (pos, {0, 0, pos.z}); + lla.x = atan2 (pos.z, p * (1 - e2)); // init latitude (rad), in +/- pi + + do + { + tmp = lla; + double N = a / sqrt (1 - e2 * sin (tmp.x) * sin (tmp.x)); + double v = p / cos (tmp.x); + lla.z = v - N; // altitude + lla.x = atan2 (pos.z, p * (1 - e2 * N / v)); + } + // 1 m difference is approx 1 / 30 arc seconds = 9.26e-6 deg + while (fabs (lla.x - tmp.x) > 0.00000926 * DEG2RAD); + + lla.x *= RAD2DEG; + lla.y *= RAD2DEG; + + // canonicalize (latitude) x in [-90, 90] and (longitude) y in [-180, 180) + if (lla.x > 90.0) + { + lla.x = 180 - lla.x; + lla.y += lla.y < 0 ? 180 : -180; + } + else if (lla.x < -90.0) + { + lla.x = -180 - lla.x; + lla.y += lla.y < 0 ? 180 : -180; + } + if (lla.y == 180.0) lla.y = -180; + + // make sure lat/lon in the right range to double check canonicalization + // and conversion routine + NS_ASSERT_MSG (-180.0 <= lla.y, "Conversion error: longitude too negative"); + NS_ASSERT_MSG (180.0 > lla.y, "Conversion error: longitude too positive"); + NS_ASSERT_MSG (-90.0 <= lla.x, "Conversion error: latitude too negative"); + NS_ASSERT_MSG (90.0 >= lla.x, "Conversion error: latitude too positive"); + + return lla; +} + std::list GeographicPositions::RandCartesianPointsAroundGeographicPoint (double originLatitude, double originLongitude, @@ -114,9 +188,9 @@ GeographicPositions::RandCartesianPointsAroundGeographicPoint (double originLati maxAltitude = 0; } - double originLatitudeRadians = originLatitude * (M_PI / 180); - double originLongitudeRadians = originLongitude * (M_PI / 180); - double originColatitude = (M_PI / 2) - originLatitudeRadians; + double originLatitudeRadians = originLatitude * DEG2RAD; + double originLongitudeRadians = originLongitude * DEG2RAD; + double originColatitude = (M_PI_2) - originLatitudeRadians; double a = maxDistFromOrigin / EARTH_RADIUS; // maximum alpha allowed // (arc length formula) @@ -138,7 +212,7 @@ GeographicPositions::RandCartesianPointsAroundGeographicPoint (double originLati // shift coordinate system from North Pole referred to origin point referred // reference: http://en.wikibooks.org/wiki/General_Astronomy/Coordinate_Systems - double theta = M_PI / 2 - alpha; // angle of elevation of new point wrt + double theta = M_PI_2 - alpha; // angle of elevation of new point wrt // origin point (latitude in coordinate // system referred to origin point) double randPointLatitude = asin(sin(theta)*cos(originColatitude) + @@ -147,12 +221,12 @@ GeographicPositions::RandCartesianPointsAroundGeographicPoint (double originLati double intermedLong = asin((sin(randPointLatitude)*cos(originColatitude) - sin(theta)) / (cos(randPointLatitude)*sin(originColatitude))); // right ascension - intermedLong = intermedLong + M_PI / 2; // shift to longitude 0 + intermedLong = intermedLong + M_PI_2; // shift to longitude 0 //flip / mirror point if it has phi in quadrant II or III (wasn't //resolved correctly by arcsin) across longitude 0 - if (phi > (M_PI / 2) && phi <= ((3 * M_PI) / 2)) - intermedLong = -intermedLong; + if (phi > (M_PI_2) && phi <= (3 * M_PI_2)) + intermedLong = -intermedLong; // shift longitude to be referenced to origin double randPointLongitude = intermedLong + originLongitudeRadians; @@ -161,8 +235,8 @@ GeographicPositions::RandCartesianPointsAroundGeographicPoint (double originLati double randAltitude = uniRand->GetValue (0, maxAltitude); Vector pointPosition = GeographicPositions::GeographicToCartesianCoordinates - (randPointLatitude * (180/M_PI), - randPointLongitude * (180/M_PI), + (randPointLatitude * RAD2DEG, + randPointLongitude * RAD2DEG, randAltitude, SPHERE); // convert coordinates diff --git a/src/mobility/model/geographic-positions.h b/src/mobility/model/geographic-positions.h index 25ebf39c0..e6f446604 100644 --- a/src/mobility/model/geographic-positions.h +++ b/src/mobility/model/geographic-positions.h @@ -73,6 +73,27 @@ public: double altitude, EarthSpheroidType sphType); +/** + * Inverse of GeographicToCartesianCoordinates using [1] + * + * This function iteratively converts cartesian (ECEF) coordinates to + * geographic coordinates. The residual delta is 1 m, which is approximately + * 1 / 30 arc seconds or 9.26e-6 deg. + * + * @param pos a vector containing the Cartesian coordinates (x, y, z referenced + * in meters) of the point (origin (0, 0, 0) is center of earth) + * @param sphType earth spheroid model to use for conversion + * + * @return Vector position where x = latitude (deg), y = longitude (deg), + * z = altitude above the ellipsoid (m) + * + * [1] "Ellipsoidal and Cartesian Coordinates Conversion", Navipedia, + * European Space Agency, Jul 8, 2019. + * + */ +static Vector CartesianToGeographicCoordinates (Vector pos, + EarthSpheroidType sphType); + /** * Generates uniformly distributed random points (in ECEF Cartesian * coordinates) within a given altitude above earth's surface centered around diff --git a/src/mobility/test/geo-to-cartesian-test.cc b/src/mobility/test/geo-to-cartesian-test.cc index 5995a45ee..d5858dc37 100644 --- a/src/mobility/test/geo-to-cartesian-test.cc +++ b/src/mobility/test/geo-to-cartesian-test.cc @@ -503,9 +503,9 @@ public: /** * Constructor * - * \param latitude latitude - * \param longitude longitude - * \param altitude altitude + * \param latitude latitude (deg) + * \param longitude longitude (deg) + * \param altitude altitude (m) * \param sphType sphere type * \param i index */ @@ -518,11 +518,12 @@ public: private: virtual void DoRun (void); + /** * Name function - * \param latitude the latitude - * \param longitude the longitude - * \param altitude the altitude + * \param latitude the latitude (deg) + * \param longitude the longitude (deg) + * \param altitude the altitude (m) * \param sphType the sphere type * \returns the name string */ @@ -530,9 +531,9 @@ private: double longitude, double altitude, GeographicPositions::EarthSpheroidType sphType); - double m_latitude; ///< latitude - double m_longitude; ///< longitude - double m_altitude; ///< altitude + double m_latitude; ///< latitude (deg) + double m_longitude; ///< longitude (deg) + double m_altitude; ///< altitude (m) GeographicPositions::EarthSpheroidType m_sphType; ///< spheroid type int m_i; ///< index }; @@ -544,10 +545,24 @@ GeoToCartesianTestCase::Name (double latitude, GeographicPositions::EarthSpheroidType sphType) { std::ostringstream oss; - oss << "latitude = " << latitude << " degrees, " - << "longitude = " << longitude << " degrees, " - << "altitude = " << altitude << " meters, " - << "earth spheroid type = " << sphType; + oss << "Geo->Cart: " + << "LAT-LON-ALT-SPHEROID = " + << latitude << " deg - " + << longitude << " deg - " + << altitude << " m - "; + switch (sphType) + { + case GeographicPositions::SPHERE: + oss << "SPHERE"; + break; + case GeographicPositions::GRS80: + oss << "GRS80"; + break; + case GeographicPositions::WGS84: + oss << "WGS84"; + break; + }; + return oss.str(); } @@ -632,6 +647,123 @@ GeoToCartesianTestCase::DoRun (void) } } +/** + * \ingroup mobility-test + * \ingroup tests + * + * \brief Cartesian to Geo Test Case + * + * This test verifies the accuracy of the CartesianToGeographicCoordinates() + * method in the GeographicPositions class, which converts earth + * ECEF Cartesian coordinates to geographic/geodetic coordinates. To do so, it + * compares the values generated from the method and converted back to geographic + * coordinates. Values are compared using 216 test cases for each of the three + * earth spheroid models. + */ +class CartesianToGeoTestCase : public TestCase +{ +public: + /** + * Constructor + * + * \param latitude latitude (deg) + * \param longitude longitude (deg) + * \param altitude altitude (m) + * \param sphType sphere type + * \param i index + */ + CartesianToGeoTestCase (double latitude, + double longitude, + double altitude, + GeographicPositions::EarthSpheroidType sphType, + int i); + virtual ~CartesianToGeoTestCase (); + +private: + virtual void DoRun (void); + + /** + * Name function + * \param latitude the latitude (deg) + * \param longitude the longitude (deg) + * \param altitude the altitude (m) + * \param sphType the sphere type + * \returns the name string + */ + static std::string Name (double latitude, + double longitude, + double altitude, + GeographicPositions::EarthSpheroidType sphType); + double m_latitude; ///< latitude (deg) + double m_longitude; ///< longitude (deg) + double m_altitude; ///< altitude (m) + GeographicPositions::EarthSpheroidType m_sphType; ///< spheroid type +}; + +std::string +CartesianToGeoTestCase::Name (double latitude, + double longitude, + double altitude, + GeographicPositions::EarthSpheroidType sphType) +{ + std::ostringstream oss; + oss << "Cart->Geo: " + << "LAT-LON-ALT-SPHEROID = " + << latitude << " deg - " + << longitude << " deg - " + << altitude << " m - "; + switch (sphType) + { + case GeographicPositions::SPHERE: + oss << "SPHERE"; + break; + case GeographicPositions::GRS80: + oss << "GRS80"; + break; + case GeographicPositions::WGS84: + oss << "WGS84"; + break; + }; + + return oss.str(); +} + +CartesianToGeoTestCase::CartesianToGeoTestCase (double latitude, + double longitude, + double altitude, + GeographicPositions::EarthSpheroidType sphType, + int i) + : TestCase (Name (latitude, longitude, altitude, sphType)), + m_latitude (latitude), + m_longitude (longitude), + m_altitude (altitude), + m_sphType (sphType) +{ +} + +CartesianToGeoTestCase::~CartesianToGeoTestCase () +{ +} + +void +CartesianToGeoTestCase::DoRun (void) +{ + Vector cart = GeographicPositions::GeographicToCartesianCoordinates (m_latitude, + m_longitude, + m_altitude, + m_sphType); + Vector geo = GeographicPositions::CartesianToGeographicCoordinates (cart, m_sphType); + + // geographic coords are ambiguous due to angular wrapping, convert to + // rectangular for comparison + Vector geocart = GeographicPositions::GeographicToCartesianCoordinates (geo.x, geo.y, geo.z, m_sphType); + + NS_TEST_ASSERT_MSG_LT_OR_EQ (CalculateDistance (cart, geocart), + 2.5, // minimum passing tolerance (m) + "Double conversion out-of-tolerance: " << + geo << " <> " << Vector ({m_latitude, m_longitude, m_altitude})); +} + /** * \ingroup mobility-test * \ingroup tests @@ -661,6 +793,12 @@ GeoToCartesianTestSuite::GeoToCartesianTestSuite () GeographicPositions::SPHERE, i), TestCase::QUICK); + AddTestCase (new CartesianToGeoTestCase (latitude, + longitude, + altitude, + GeographicPositions::SPHERE, + i), + TestCase::QUICK); ++i; } } @@ -678,6 +816,12 @@ GeoToCartesianTestSuite::GeoToCartesianTestSuite () GeographicPositions::GRS80, i), TestCase::QUICK); + AddTestCase (new CartesianToGeoTestCase (latitude, + longitude, + altitude, + GeographicPositions::GRS80, + i), + TestCase::QUICK); ++i; } } @@ -695,6 +839,12 @@ GeoToCartesianTestSuite::GeoToCartesianTestSuite () GeographicPositions::WGS84, i), TestCase::QUICK); + AddTestCase (new CartesianToGeoTestCase (latitude, + longitude, + altitude, + GeographicPositions::WGS84, + i), + TestCase::QUICK); ++i; } }