F5/unison
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

mobility: (merges !101) Add CartesianToGeographic conversion with test

Browse Source 
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.
This commit is contained in:
Jared Dulmage
2019-09-03 10:27:13 -06:00
committed by Tom Henderson
parent 7514b453f0
commit 19a5e0de30
3 changed files with 273 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
src/mobility/model/geographic-positions.cc
View File

@@ -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<Vector>
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

21
src/mobility/model/geographic-positions.h
View File

@@ -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.
* <https://gssc.esa.int/navipedia/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion>
*/
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

176
src/mobility/test/geo-to-cartesian-test.cc
View File

@@ -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;
}
}