Skip to content

Commit

Permalink
added wrapGeodetic
Browse files Browse the repository at this point in the history
  • Loading branch information
@ClancyWalters
ClancyWalters authored Mar 19, 2024
2 parents c45bcb4 + 51a426f commit 9d59050
Show file tree
Hide file tree
Showing 2 changed files with 166 additions and 0 deletions.
49 changes: 49 additions & 0 deletions src/cppmap3d.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1136,6 +1136,55 @@ inline void ned2aer(
) {
enu2aer(east, north, -1 * down, out_az, out_el, out_range);
}

/*
* @brief Normalize co-ordinates that lie outside of the normal ranges.
*
* Longitude wrapping simply requires adding +- 360 to the value until it comes
* into range.
*
* For the latitude values, we need to flip the longitude whenever the latitude
* a pole.
*
* @param lat The latitude, in radians.
* @param lon The longitude, in radians.
* @param[out] out_lat The latitude, in radians (output parameter).
* @param[out] out_lon The longitude, in radians (output parameter).
*
* Ported from https://github.com/pelias/api/blob/8f6596db27ef74a451e4307f81a0ff7e7870ac03/sanitizer/wrap.js#L10 (MIT licence)
*/

inline void wrapGeodetic(double lat, double lon, double& out_lat, double& out_lon) {

const double pi = 3.14159265358979311599796346854;
int quadrant = static_cast<int>(floor(abs(lat) / (pi/2))) % 4;
double pole = (lat > 0) ? (pi / 2) : -(pi / 2);
double offset = std::fmod(lat, (pi / 2));

switch (quadrant) {
case 0:
out_lat = offset;
out_lon = lon;
break;
case 1:
out_lat = pole - offset;
out_lon = lon + pi;
break;
case 2:
out_lat = -offset;
out_lon = lon + pi;
break;
case 3:
out_lat = -pole + offset;
out_lon = lon;
break;
}

if (out_lon > pi || out_lon < pi) {
out_lon -= floor((out_lon + pi) / (pi * 2)) * (pi * 2);
}
}

} // namespace cppmap3d

#endif
117 changes: 117 additions & 0 deletions src/tests/cppmap3d_tests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -541,3 +541,120 @@ TEST_CASE("ecef2ned") {
CHECK(n == doctest::Approx(nref));
CHECK(d == doctest::Approx(dref));
}

TEST_CASE("wrapGeodetic") {

// Control
double lat, lon;
cppmap3d::wrapGeodetic(radians(55.555), radians(22.222), lat, lon);
CHECK(lat == doctest::Approx(radians(55.555)));
CHECK(lon == doctest::Approx(radians(22.222)));

// Positive latitude
cppmap3d::wrapGeodetic(radians(1), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(1)));
CHECK(lon == doctest::Approx(radians(0)));

cppmap3d::wrapGeodetic(radians(91), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(89)));
CHECK(lon == doctest::Approx(radians(180)));

cppmap3d::wrapGeodetic(radians(181), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(-1)));
CHECK(lon == doctest::Approx(radians(180)));

cppmap3d::wrapGeodetic(radians(271), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(-89)));
CHECK(lon == doctest::Approx(radians(0)));

cppmap3d::wrapGeodetic(radians(361), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(1)));
CHECK(lon == doctest::Approx(radians(0)));

cppmap3d::wrapGeodetic(radians(631), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(-89)));
CHECK(lon == doctest::Approx(radians(0)));

cppmap3d::wrapGeodetic(radians(721), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(1)));
CHECK(lon == doctest::Approx(radians(0)));

// Negative Latitude
cppmap3d::wrapGeodetic(radians(-1), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(-1)));
CHECK(lon == doctest::Approx(radians(0)));

cppmap3d::wrapGeodetic(radians(-91), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(-89)));
CHECK(lon == doctest::Approx(radians(180)));

cppmap3d::wrapGeodetic(radians(-181), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(1)));
CHECK(lon == doctest::Approx(radians(180)));

cppmap3d::wrapGeodetic(radians(-271), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(89)));
CHECK(lon == doctest::Approx(radians(0)));

cppmap3d::wrapGeodetic(radians(-361), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(-1)));
CHECK(lon == doctest::Approx(radians(0)));

cppmap3d::wrapGeodetic(radians(-631), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(89)));
CHECK(lon == doctest::Approx(radians(0)));

cppmap3d::wrapGeodetic(radians(-721), radians(0), lat, lon);
CHECK(lat == doctest::Approx(radians(-1)));
CHECK(lon == doctest::Approx(radians(0)));

// Positive Longitude
cppmap3d::wrapGeodetic(radians(0), radians(1), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(1)));

cppmap3d::wrapGeodetic(radians(0), radians(181), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(-179)));

cppmap3d::wrapGeodetic(radians(0), radians(271), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(-89)));

cppmap3d::wrapGeodetic(radians(0), radians(361), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(1)));

cppmap3d::wrapGeodetic(radians(0), radians(631), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(-89)));

cppmap3d::wrapGeodetic(radians(0), radians(721), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(1)));

// Negative Longitude
cppmap3d::wrapGeodetic(radians(0), radians(-1), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(-1)));

cppmap3d::wrapGeodetic(radians(0), radians(-181), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(179)));

cppmap3d::wrapGeodetic(radians(0), radians(-271), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(89)));

cppmap3d::wrapGeodetic(radians(0), radians(-361), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(-1)));

cppmap3d::wrapGeodetic(radians(0), radians(-631), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(89)));

cppmap3d::wrapGeodetic(radians(0), radians(-721), lat, lon);
CHECK(lat == doctest::Approx(radians(0)));
CHECK(lon == doctest::Approx(radians(-1)));
}

0 comments on commit 9d59050

Please sign in to comment.