From d2654a9ceb3f8eb52d5e8f13aabe82f6a399c447 Mon Sep 17 00:00:00 2001 From: Alex Date: Thu, 13 Feb 2020 02:38:26 +0700 Subject: [PATCH] Readability improved --- README.md | 12 +- include/LatLng.hpp | 14 +- include/MathUtil.hpp | 182 +++--- include/PolyUtil.hpp | 542 +++++++++--------- include/SphericalUtil.hpp | 430 +++++++------- tests/SphericalUtil/computeAngleBetween.hpp | 54 +- tests/SphericalUtil/computeArea.hpp | 10 +- .../SphericalUtil/computeDistanceBetween.hpp | 6 +- tests/SphericalUtil/computeHeading.hpp | 40 +- tests/SphericalUtil/computeOffset.hpp | 30 +- tests/SphericalUtil/computeOffsetOrigin.hpp | 24 +- tests/SphericalUtil/interpolate.hpp | 1 - 12 files changed, 672 insertions(+), 673 deletions(-) diff --git a/README.md b/README.md index bb35ec2..e290e5d 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,6 @@ C++ Geometry Library provides utility functions for the computation of geometric * [Spherical](https://developers.google.com/maps/documentation/javascript/reference#spherical) contains spherical geometry utilities allowing you to compute angles, distances and areas from latitudes and longitudes. * [Poly](https://developers.google.com/maps/documentation/javascript/reference#poly) utility functions for computations involving polygons and polylines. -* [Encoding](https://developers.google.com/maps/documentation/javascript/reference#encoding) utilities for polyline encoding and decoding. ## Usage @@ -66,9 +65,9 @@ int main() { ### PolyUtil class -* [`containsLocation(LatLng point, LatLngList polygon, bool geodesic = false)`](#containsLocation) -* [`isLocationOnEdge(LatLng point, LatLngList polygon, double tolerance = PolyUtil::DEFAULT_TOLERANCE, bool geodesic = true)`](#isLocationOnEdge) -* [`isLocationOnPath(LatLng point, LatLngList polyline, double tolerance = PolyUtil::DEFAULT_TOLERANCE, bool geodesic = true)`](#isLocationOnPath) +* [`containsLocation(LatLng point, LatLngList polygon, bool geodesic)`](#containsLocation) +* [`isLocationOnEdge(LatLng point, LatLngList polygon, double tolerance, bool geodesic)`](#isLocationOnEdge) +* [`isLocationOnPath(LatLng point, LatLngList polyline, double tolerance, bool geodesic)`](#isLocationOnPath) * [`distanceToLine(LatLng point, LatLng start, LatLng end)`](#distanceToLine) ### SphericalUtil class @@ -360,6 +359,5 @@ assert(SphericalUtil::computeSignedArea(path) == -SphericalUtil::computeSignedAr ## License -Geometry Library Google Maps API V3 is released under the MIT License. See the bundled -[LICENSE](https://github.com/alexpechkarev/geometry-library/blob/master/LICENSE) -file for details. +Geometry Library Google Maps API V3 is released under the MIT License. +See the bundled [LICENSE](https://github.com/alexpechkarev/geometry-library/blob/master/LICENSE) file for details. \ No newline at end of file diff --git a/include/LatLng.hpp b/include/LatLng.hpp index 0c34f0c..6b2b9ca 100644 --- a/include/LatLng.hpp +++ b/include/LatLng.hpp @@ -18,8 +18,8 @@ class LatLng { public: - double lat; // The latitude of this location - double lng; // The longitude of this location + double lat; // The latitude of this location + double lng; // The longitude of this location /** * Constructs a location with a latitude/longitude pair. @@ -30,10 +30,14 @@ class LatLng { LatLng(double lat, double lng) : lat(lat), lng(lng) {} - bool operator==(const LatLng& other) { - return isCoordinateEqual(lat, other.lat) && + LatLng(const LatLng & point) = default; + + LatLng& operator=(const LatLng & other) = default; + + bool operator==(const LatLng & other) { + return isCoordinateEqual(lat, other.lat) && isCoordinateEqual(lng, other.lng); - } + } private: diff --git a/include/MathUtil.hpp b/include/MathUtil.hpp index f460a77..88e7896 100644 --- a/include/MathUtil.hpp +++ b/include/MathUtil.hpp @@ -21,104 +21,104 @@ #define M_PI 3.14159265358979323846 inline double deg2rad(double degrees) { - return degrees * M_PI / 180.0; + return degrees * M_PI / 180.0; } inline double rad2deg(double angle) { - return angle * 180.0 / M_PI; + return angle * 180.0 / M_PI; } class MathUtil { public: - /** - * The earth's radius, in meters. - * Mean radius as defined by IUGG. - */ - static constexpr double EARTH_RADIUS = 6371009.0; - - /** - * Restrict x to the range [low, high]. - */ - static inline double clamp(double x, double low, double high) { - return x < low ? low : (x > high ? high : x); - } - - /** - * Wraps the given value into the inclusive-exclusive interval between min and max. - * @param n The value to wrap. - * @param min The minimum. - * @param max The maximum. - */ - static inline double wrap(double n, double min, double max) { - return (n >= min && n < max) ? n : (MathUtil::mod(n - min, max - min) + min); - } - - /** - * Returns the non-negative remainder of x / m. - * @param x The operand. - * @param m The modulus. - */ - static inline double mod(double x, double m) { - return remainder(remainder(x, m) + m, m); - } - - /** - * Returns mercator Y corresponding to latitude. - * See http://en.wikipedia.org/wiki/Mercator_projection . - */ - static inline double mercator(double lat) { - return log(tan(lat * 0.5 + M_PI / 4.0)); - } - - /** - * Returns latitude from mercator Y. - */ - static inline double inverseMercator(double y) { - return 2.0 * atan(exp(y)) - M_PI / 2.0; - } - - /** - * Returns haversine(angle-in-radians). - * hav(x) == (1 - cos(x)) / 2 == sin(x / 2)^2. - */ - static inline double hav(double x) { - double sinHalf = sin(x * 0.5); - return sinHalf * sinHalf; - } - - /** - * Computes inverse haversine. Has good numerical stability around 0. - * arcHav(x) == acos(1 - 2 * x) == 2 * asin(sqrt(x)). - * The argument must be in [0, 1], and the result is positive. - */ - static inline double arcHav(double x) { - return 2.0 * asin(sqrt(x)); - } - - // Given h==hav(x), returns sin(abs(x)). - static inline double sinFromHav(double h) { - return 2.0 * sqrt(h * (1.0 - h)); - } - - // Returns hav(asin(x)). - static inline double havFromSin(double x) { - double x2 = x * x; - return x2 / (1.0 + sqrt(1.0 - x2)) * 0.5; - } - - // Returns sin(arcHav(x) + arcHav(y)). - static inline double sinSumFromHav(double x, double y) { - double a = sqrt(x * (1 - x)); - double b = sqrt(y * (1 - y)); - return 2.0 * (a + b - 2 * (a * y + b * x)); - } - - /** - * Returns hav() of distance from (lat1, lng1) to (lat2, lng2) on the unit sphere. - */ - static inline double havDistance(double lat1, double lat2, double dLng) { - return MathUtil::hav(lat1 - lat2) + MathUtil::hav(dLng) * cos(lat1) * cos(lat2); - } + /** + * The earth's radius, in meters. + * Mean radius as defined by IUGG. + */ + static constexpr double EARTH_RADIUS = 6371009.0; + + /** + * Restrict x to the range [low, high]. + */ + static inline double clamp(double x, double low, double high) { + return x < low ? low : (x > high ? high : x); + } + + /** + * Wraps the given value into the inclusive-exclusive interval between min and max. + * @param n The value to wrap. + * @param min The minimum. + * @param max The maximum. + */ + static inline double wrap(double n, double min, double max) { + return (n >= min && n < max) ? n : (MathUtil::mod(n - min, max - min) + min); + } + + /** + * Returns the non-negative remainder of x / m. + * @param x The operand. + * @param m The modulus. + */ + static inline double mod(double x, double m) { + return remainder(remainder(x, m) + m, m); + } + + /** + * Returns mercator Y corresponding to latitude. + * See http://en.wikipedia.org/wiki/Mercator_projection . + */ + static inline double mercator(double lat) { + return log(tan(lat * 0.5 + M_PI / 4.0)); + } + + /** + * Returns latitude from mercator Y. + */ + static inline double inverseMercator(double y) { + return 2.0 * atan(exp(y)) - M_PI / 2.0; + } + + /** + * Returns haversine(angle-in-radians). + * hav(x) == (1 - cos(x)) / 2 == sin(x / 2)^2. + */ + static inline double hav(double x) { + double sinHalf = sin(x * 0.5); + return sinHalf * sinHalf; + } + + /** + * Computes inverse haversine. Has good numerical stability around 0. + * arcHav(x) == acos(1 - 2 * x) == 2 * asin(sqrt(x)). + * The argument must be in [0, 1], and the result is positive. + */ + static inline double arcHav(double x) { + return 2.0 * asin(sqrt(x)); + } + + // Given h==hav(x), returns sin(abs(x)). + static inline double sinFromHav(double h) { + return 2.0 * sqrt(h * (1.0 - h)); + } + + // Returns hav(asin(x)). + static inline double havFromSin(double x) { + double x2 = x * x; + return x2 / (1.0 + sqrt(1.0 - x2)) * 0.5; + } + + // Returns sin(arcHav(x) + arcHav(y)). + static inline double sinSumFromHav(double x, double y) { + double a = sqrt(x * (1 - x)); + double b = sqrt(y * (1 - y)); + return 2.0 * (a + b - 2 * (a * y + b * x)); + } + + /** + * Returns hav() of distance from (lat1, lng1) to (lat2, lng2) on the unit sphere. + */ + static inline double havDistance(double lat1, double lat2, double dLng) { + return MathUtil::hav(lat1 - lat2) + MathUtil::hav(dLng) * cos(lat1) * cos(lat2); + } }; #endif // GEOMETRY_LIBRARY_MATH_UTIL diff --git a/include/PolyUtil.hpp b/include/PolyUtil.hpp index 23438dc..d446321 100644 --- a/include/PolyUtil.hpp +++ b/include/PolyUtil.hpp @@ -21,301 +21,301 @@ class PolyUtil { public: - static constexpr double DEFAULT_TOLERANCE = 0.1; // meters + static constexpr double DEFAULT_TOLERANCE = 0.1; // meters - /** - * Computes whether the given point lies inside the specified polygon. - * The polygon is always cosidered closed, regardless of whether the last point equals - * the first or not. - * Inside is defined as not containing the South Pole -- the South Pole is always outside. - * The polygon is formed of great circle segments if geodesic is true, and of rhumb - * (loxodromic) segments otherwise. - */ - template - static inline bool containsLocation(LatLng point, LatLngList polygon, bool geodesic = false) { - size_t size = polygon.size(); + /** + * Computes whether the given point lies inside the specified polygon. + * The polygon is always cosidered closed, regardless of whether the last point equals + * the first or not. + * Inside is defined as not containing the South Pole -- the South Pole is always outside. + * The polygon is formed of great circle segments if geodesic is true, and of rhumb + * (loxodromic) segments otherwise. + */ + template + static inline bool containsLocation(LatLng point, LatLngList polygon, bool geodesic = false) { + size_t size = polygon.size(); - if (size == 0) { - return false; - } - double lat3 = deg2rad(point.lat); - double lng3 = deg2rad(point.lng); - LatLng prev = polygon[size - 1]; - double lat1 = deg2rad(prev.lat); - double lng1 = deg2rad(prev.lng); + if (size == 0) { + return false; + } + double lat3 = deg2rad(point.lat); + double lng3 = deg2rad(point.lng); + LatLng prev = polygon[size - 1]; + double lat1 = deg2rad(prev.lat); + double lng1 = deg2rad(prev.lng); - size_t nIntersect = 0; + size_t nIntersect = 0; - for (auto val : polygon) { - double dLng3 = MathUtil::wrap(lng3 - lng1, -M_PI, M_PI); - // Special case: point equal to vertex is inside. - if (lat3 == lat1 && dLng3 == 0) { - return true; - } + for (auto val : polygon) { + double dLng3 = MathUtil::wrap(lng3 - lng1, -M_PI, M_PI); + // Special case: point equal to vertex is inside. + if (lat3 == lat1 && dLng3 == 0) { + return true; + } - double lat2 = deg2rad(val.lat); - double lng2 = deg2rad(val.lng); + double lat2 = deg2rad(val.lat); + double lng2 = deg2rad(val.lng); - // Offset longitudes by -lng1. - if (PolyUtil::intersects(lat1, lat2, MathUtil::wrap(lng2 - lng1, -M_PI, M_PI), lat3, dLng3, geodesic)) { - ++nIntersect; - } - lat1 = lat2; - lng1 = lng2; - } - return (nIntersect & 1) != 0; - } + // Offset longitudes by -lng1. + if (PolyUtil::intersects(lat1, lat2, MathUtil::wrap(lng2 - lng1, -M_PI, M_PI), lat3, dLng3, geodesic)) { + ++nIntersect; + } + lat1 = lat2; + lng1 = lng2; + } + return (nIntersect & 1) != 0; + } - /** - * Computes whether the given point lies on or near the edge of a polygon, within a specified - * tolerance in meters. The polygon edge is composed of great circle segments if geodesic - * is true, and of Rhumb segments otherwise. The polygon edge is implicitly closed -- the - * closing segment between the first point and the last point is included. - */ - template - static inline bool isLocationOnEdge(LatLng point, LatLngList polygon, double tolerance = PolyUtil::DEFAULT_TOLERANCE, bool geodesic = true) { - return PolyUtil::isLocationOnEdgeOrPath(point, polygon, true, geodesic, tolerance); - } + /** + * Computes whether the given point lies on or near the edge of a polygon, within a specified + * tolerance in meters. The polygon edge is composed of great circle segments if geodesic + * is true, and of Rhumb segments otherwise. The polygon edge is implicitly closed -- the + * closing segment between the first point and the last point is included. + */ + template + static inline bool isLocationOnEdge(LatLng point, LatLngList polygon, double tolerance = PolyUtil::DEFAULT_TOLERANCE, bool geodesic = true) { + return PolyUtil::isLocationOnEdgeOrPath(point, polygon, true, geodesic, tolerance); + } - /** - * Computes whether the given point lies on or near a polyline, within a specified - * tolerance in meters. The polyline is composed of great circle segments if geodesic - * is true, and of Rhumb segments otherwise. The polyline is not closed -- the closing - * segment between the first point and the last point is not included. - */ - template - static inline bool isLocationOnPath(LatLng point, LatLngList polyline, double tolerance = PolyUtil::DEFAULT_TOLERANCE, bool geodesic = true) { - return PolyUtil::isLocationOnEdgeOrPath(point, polyline, false, geodesic, tolerance); - } + /** + * Computes whether the given point lies on or near a polyline, within a specified + * tolerance in meters. The polyline is composed of great circle segments if geodesic + * is true, and of Rhumb segments otherwise. The polyline is not closed -- the closing + * segment between the first point and the last point is not included. + */ + template + static inline bool isLocationOnPath(LatLng point, LatLngList polyline, double tolerance = PolyUtil::DEFAULT_TOLERANCE, bool geodesic = true) { + return PolyUtil::isLocationOnEdgeOrPath(point, polyline, false, geodesic, tolerance); + } - /** - * Computes whether (and where) a given point lies on or near a polyline, within a specified tolerance. - * If closed, the closing segment between the last and first points of the polyline is not considered. - * - * @param point our needle - * @param poly our haystack - * @param closed whether the polyline should be considered closed by a segment connecting the last point back to the first one - * @param geodesic the polyline is composed of great circle segments if geodesic - * is true, and of Rhumb segments otherwise - * @param toleranceEarth tolerance (in meters) - * @return -1 if point does not lie on or near the polyline. - * 0 if point is between poly[0] and poly[1] (inclusive), - * 1 if between poly[1] and poly[2], - * ..., - * poly.size()-2 if between poly[poly.size() - 2] and poly[poly.size() - 1] - */ - template - static inline bool isLocationOnEdgeOrPath(LatLng point, LatLngList poly, bool closed, bool geodesic, double toleranceEarth) { - size_t size = poly.size(); + /** + * Computes whether (and where) a given point lies on or near a polyline, within a specified tolerance. + * If closed, the closing segment between the last and first points of the polyline is not considered. + * + * @param point our needle + * @param poly our haystack + * @param closed whether the polyline should be considered closed by a segment connecting the last point back to the first one + * @param geodesic the polyline is composed of great circle segments if geodesic + * is true, and of Rhumb segments otherwise + * @param toleranceEarth tolerance (in meters) + * @return -1 if point does not lie on or near the polyline. + * 0 if point is between poly[0] and poly[1] (inclusive), + * 1 if between poly[1] and poly[2], + * ..., + * poly.size()-2 if between poly[poly.size() - 2] and poly[poly.size() - 1] + */ + template + static inline bool isLocationOnEdgeOrPath(LatLng point, LatLngList poly, bool closed, bool geodesic, double toleranceEarth) { + size_t size = poly.size(); - if (size == 0U) { - return false; - } + if (size == 0U) { + return false; + } - double tolerance = toleranceEarth / MathUtil::EARTH_RADIUS; - double havTolerance = MathUtil::hav(tolerance); - double lat3 = deg2rad(point.lat); - double lng3 = deg2rad(point.lng); - LatLng prev = poly[closed ? size - 1 : 0]; - double lat1 = deg2rad(prev.lat); - double lng1 = deg2rad(prev.lng); + double tolerance = toleranceEarth / MathUtil::EARTH_RADIUS; + double havTolerance = MathUtil::hav(tolerance); + double lat3 = deg2rad(point.lat); + double lng3 = deg2rad(point.lng); + LatLng prev = poly[closed ? size - 1 : 0]; + double lat1 = deg2rad(prev.lat); + double lng1 = deg2rad(prev.lng); - if (geodesic) { - for (auto val : poly) { - double lat2 = deg2rad(val.lat); - double lng2 = deg2rad(val.lng); - if (PolyUtil::isOnSegmentGC(lat1, lng1, lat2, lng2, lat3, lng3, havTolerance)) { - return true; - } - lat1 = lat2; - lng1 = lng2; - } - }else { - // We project the points to mercator space, where the Rhumb segment is a straight line, - // and compute the geodesic distance between point3 and the closest point on the - // segment. This method is an approximation, because it uses "closest" in mercator - // space which is not "closest" on the sphere -- but the error is small because - // "tolerance" is small. - double minAcceptable = lat3 - tolerance; - double maxAcceptable = lat3 + tolerance; - double y1 = MathUtil::mercator(lat1); - double y3 = MathUtil::mercator(lat3); - double xTry[3]; - for (auto val : poly) { - double lat2 = deg2rad(val.lat); - double y2 = MathUtil::mercator(lat2); - double lng2 = deg2rad(val.lng); - if (std::max(lat1, lat2) >= minAcceptable && std::min(lat1, lat2) <= maxAcceptable) { - // We offset longitudes by -lng1; the implicit x1 is 0. - double x2 = MathUtil::wrap(lng2 - lng1, -M_PI, M_PI); - double x3Base = MathUtil::wrap(lng3 - lng1, -M_PI, M_PI); - xTry[0] = x3Base; - // Also explore wrapping of x3Base around the world in both directions. - xTry[1] = x3Base + 2 * M_PI; - xTry[2] = x3Base - 2 * M_PI; + if (geodesic) { + for (auto val : poly) { + double lat2 = deg2rad(val.lat); + double lng2 = deg2rad(val.lng); + if (PolyUtil::isOnSegmentGC(lat1, lng1, lat2, lng2, lat3, lng3, havTolerance)) { + return true; + } + lat1 = lat2; + lng1 = lng2; + } + }else { + // We project the points to mercator space, where the Rhumb segment is a straight line, + // and compute the geodesic distance between point3 and the closest point on the + // segment. This method is an approximation, because it uses "closest" in mercator + // space which is not "closest" on the sphere -- but the error is small because + // "tolerance" is small. + double minAcceptable = lat3 - tolerance; + double maxAcceptable = lat3 + tolerance; + double y1 = MathUtil::mercator(lat1); + double y3 = MathUtil::mercator(lat3); + double xTry[3]; + for (auto val : poly) { + double lat2 = deg2rad(val.lat); + double y2 = MathUtil::mercator(lat2); + double lng2 = deg2rad(val.lng); + if (std::max(lat1, lat2) >= minAcceptable && std::min(lat1, lat2) <= maxAcceptable) { + // We offset longitudes by -lng1; the implicit x1 is 0. + double x2 = MathUtil::wrap(lng2 - lng1, -M_PI, M_PI); + double x3Base = MathUtil::wrap(lng3 - lng1, -M_PI, M_PI); + xTry[0] = x3Base; + // Also explore wrapping of x3Base around the world in both directions. + xTry[1] = x3Base + 2 * M_PI; + xTry[2] = x3Base - 2 * M_PI; - for (auto x3 : xTry) { - double dy = y2 - y1; - double len2 = x2 * x2 + dy * dy; - double t = len2 <= 0 ? 0 : MathUtil::clamp((x3 * x2 + (y3 - y1) * dy) / len2, 0, 1); - double xClosest = t * x2; - double yClosest = y1 + t * dy; - double latClosest = MathUtil::inverseMercator(yClosest); - double havDist = MathUtil::havDistance(lat3, latClosest, x3 - xClosest); - if (havDist < havTolerance) { - return true; - } - } - } - lat1 = lat2; - lng1 = lng2; - y1 = y2; - } - } - return false; - } + for (auto x3 : xTry) { + double dy = y2 - y1; + double len2 = x2 * x2 + dy * dy; + double t = len2 <= 0 ? 0 : MathUtil::clamp((x3 * x2 + (y3 - y1) * dy) / len2, 0, 1); + double xClosest = t * x2; + double yClosest = y1 + t * dy; + double latClosest = MathUtil::inverseMercator(yClosest); + double havDist = MathUtil::havDistance(lat3, latClosest, x3 - xClosest); + if (havDist < havTolerance) { + return true; + } + } + } + lat1 = lat2; + lng1 = lng2; + y1 = y2; + } + } + return false; + } - /** - * Computes the distance on the sphere between the point p and the line segment start to end. - * - * @param p the point to be measured - * @param start the beginning of the line segment - * @param end the end of the line segment - * @return the distance in meters (assuming spherical earth) - */ - static inline double distanceToLine(LatLng p, LatLng start, LatLng end) { - if (start == end) { - return SphericalUtil::computeDistanceBetween(end, p); - } - double s0lat = deg2rad(p.lat); - double s0lng = deg2rad(p.lng); - double s1lat = deg2rad(start.lat); - double s1lng = deg2rad(start.lng); - double s2lat = deg2rad(end.lat); - double s2lng = deg2rad(end.lng); - double s2s1lat = s2lat - s1lat; - double s2s1lng = s2lng - s1lng; - double u = ((s0lat - s1lat) * s2s1lat + (s0lng - s1lng) * s2s1lng) - / (s2s1lat * s2s1lat + s2s1lng * s2s1lng); - if (u <= 0) { - return SphericalUtil::computeDistanceBetween(p, start); - } - if (u >= 1) { - return SphericalUtil::computeDistanceBetween(p, end); - } - LatLng su(start.lat + u * (end.lat - start.lat), start.lng + u * (end.lng - start.lng)); + /** + * Computes the distance on the sphere between the point p and the line segment start to end. + * + * @param p the point to be measured + * @param start the beginning of the line segment + * @param end the end of the line segment + * @return the distance in meters (assuming spherical earth) + */ + static inline double distanceToLine(LatLng p, LatLng start, LatLng end) { + if (start == end) { + return SphericalUtil::computeDistanceBetween(end, p); + } + double s0lat = deg2rad(p.lat); + double s0lng = deg2rad(p.lng); + double s1lat = deg2rad(start.lat); + double s1lng = deg2rad(start.lng); + double s2lat = deg2rad(end.lat); + double s2lng = deg2rad(end.lng); + double s2s1lat = s2lat - s1lat; + double s2s1lng = s2lng - s1lng; + double u = ((s0lat - s1lat) * s2s1lat + (s0lng - s1lng) * s2s1lng) + / (s2s1lat * s2s1lat + s2s1lng * s2s1lng); + if (u <= 0) { + return SphericalUtil::computeDistanceBetween(p, start); + } + if (u >= 1) { + return SphericalUtil::computeDistanceBetween(p, end); + } + LatLng su(start.lat + u * (end.lat - start.lat), start.lng + u * (end.lng - start.lng)); return SphericalUtil::computeDistanceBetween(p, su); - } + } private: - /** - * Returns tan(latitude-at-lng3) on the great circle (lat1, lng1) to (lat2, lng2). lng1==0. - * See http://williams.best.vwh.net/avform.htm . - */ - static inline double tanLatGC(double lat1, double lat2, double lng2, double lng3) { - return (tan(lat1) * sin(lng2 - lng3) + tan(lat2) * sin(lng3)) / sin(lng2); - } + /** + * Returns tan(latitude-at-lng3) on the great circle (lat1, lng1) to (lat2, lng2). lng1==0. + * See http://williams.best.vwh.net/avform.htm . + */ + static inline double tanLatGC(double lat1, double lat2, double lng2, double lng3) { + return (tan(lat1) * sin(lng2 - lng3) + tan(lat2) * sin(lng3)) / sin(lng2); + } - /** - * Returns mercator(latitude-at-lng3) on the Rhumb line (lat1, lng1) to (lat2, lng2). lng1==0. - */ - static inline double mercatorLatRhumb(double lat1, double lat2, double lng2, double lng3) { - return (MathUtil::mercator(lat1) * (lng2 - lng3) + MathUtil::mercator(lat2) * lng3) / lng2; - } + /** + * Returns mercator(latitude-at-lng3) on the Rhumb line (lat1, lng1) to (lat2, lng2). lng1==0. + */ + static inline double mercatorLatRhumb(double lat1, double lat2, double lng2, double lng3) { + return (MathUtil::mercator(lat1) * (lng2 - lng3) + MathUtil::mercator(lat2) * lng3) / lng2; + } - /** - * Computes whether the vertical segment (lat3, lng3) to South Pole intersects the segment - * (lat1, lng1) to (lat2, lng2). - * Longitudes are offset by -lng1; the implicit lng1 becomes 0. - */ - static inline double intersects(double lat1, double lat2, double lng2, double lat3, double lng3, bool geodesic) { - // Both ends on the same side of lng3. - if ((lng3 >= 0 && lng3 >= lng2) || (lng3 < 0 && lng3 < lng2)) { - return false; - } - // Point is South Pole. - if (lat3 <= -M_PI / 2) { - return false; - } - // Any segment end is a pole. - if (lat1 <= -M_PI / 2 || lat2 <= -M_PI / 2 || lat1 >= M_PI / 2 || lat2 >= M_PI / 2) { - return false; - } - if (lng2 <= -M_PI) { - return false; - } - double linearLat = (lat1 * (lng2 - lng3) + lat2 * lng3) / lng2; - // Northern hemisphere and point under lat-lng line. - if (lat1 >= 0 && lat2 >= 0 && lat3 < linearLat) { - return false; - } - // Southern hemisphere and point above lat-lng line. - if (lat1 <= 0 && lat2 <= 0 && lat3 >= linearLat) { - return true; - } - // North Pole. - if (lat3 >= M_PI / 2) { - return true; - } - // Compare lat3 with latitude on the GC/Rhumb segment corresponding to lng3. - // Compare through a strictly-increasing function (tan() or mercator()) as convenient. - return geodesic ? - tan(lat3) >= PolyUtil::tanLatGC(lat1, lat2, lng2, lng3) : - MathUtil::mercator(lat3) >= PolyUtil::mercatorLatRhumb(lat1, lat2, lng2, lng3); - } + /** + * Computes whether the vertical segment (lat3, lng3) to South Pole intersects the segment + * (lat1, lng1) to (lat2, lng2). + * Longitudes are offset by -lng1; the implicit lng1 becomes 0. + */ + static inline double intersects(double lat1, double lat2, double lng2, double lat3, double lng3, bool geodesic) { + // Both ends on the same side of lng3. + if ((lng3 >= 0 && lng3 >= lng2) || (lng3 < 0 && lng3 < lng2)) { + return false; + } + // Point is South Pole. + if (lat3 <= -M_PI / 2) { + return false; + } + // Any segment end is a pole. + if (lat1 <= -M_PI / 2 || lat2 <= -M_PI / 2 || lat1 >= M_PI / 2 || lat2 >= M_PI / 2) { + return false; + } + if (lng2 <= -M_PI) { + return false; + } + double linearLat = (lat1 * (lng2 - lng3) + lat2 * lng3) / lng2; + // Northern hemisphere and point under lat-lng line. + if (lat1 >= 0 && lat2 >= 0 && lat3 < linearLat) { + return false; + } + // Southern hemisphere and point above lat-lng line. + if (lat1 <= 0 && lat2 <= 0 && lat3 >= linearLat) { + return true; + } + // North Pole. + if (lat3 >= M_PI / 2) { + return true; + } + // Compare lat3 with latitude on the GC/Rhumb segment corresponding to lng3. + // Compare through a strictly-increasing function (tan() or mercator()) as convenient. + return geodesic ? + tan(lat3) >= PolyUtil::tanLatGC(lat1, lat2, lng2, lng3) : + MathUtil::mercator(lat3) >= PolyUtil::mercatorLatRhumb(lat1, lat2, lng2, lng3); + } - /** - * Returns sin(initial bearing from (lat1,lng1) to (lat3,lng3) minus initial bearing - * from (lat1, lng1) to (lat2,lng2)). - */ - static inline double sinDeltaBearing(double lat1, double lng1, double lat2, double lng2, double lat3, double lng3) { - double sinLat1 = sin(lat1); - double cosLat2 = cos(lat2); - double cosLat3 = cos(lat3); - double lat31 = lat3 - lat1; - double lng31 = lng3 - lng1; - double lat21 = lat2 - lat1; - double lng21 = lng2 - lng1; - double a = sin(lng31) * cosLat3; - double c = sin(lng21) * cosLat2; - double b = sin(lat31) + 2 * sinLat1 * cosLat3 * MathUtil::hav(lng31); - double d = sin(lat21) + 2 * sinLat1 * cosLat2 * MathUtil::hav(lng21); - double denom = (a * a + b * b) * (c * c + d * d); - return denom <= 0 ? 1 : (a * d - b * c) / sqrt(denom); - } + /** + * Returns sin(initial bearing from (lat1,lng1) to (lat3,lng3) minus initial bearing + * from (lat1, lng1) to (lat2,lng2)). + */ + static inline double sinDeltaBearing(double lat1, double lng1, double lat2, double lng2, double lat3, double lng3) { + double sinLat1 = sin(lat1); + double cosLat2 = cos(lat2); + double cosLat3 = cos(lat3); + double lat31 = lat3 - lat1; + double lng31 = lng3 - lng1; + double lat21 = lat2 - lat1; + double lng21 = lng2 - lng1; + double a = sin(lng31) * cosLat3; + double c = sin(lng21) * cosLat2; + double b = sin(lat31) + 2 * sinLat1 * cosLat3 * MathUtil::hav(lng31); + double d = sin(lat21) + 2 * sinLat1 * cosLat2 * MathUtil::hav(lng21); + double denom = (a * a + b * b) * (c * c + d * d); + return denom <= 0 ? 1 : (a * d - b * c) / sqrt(denom); + } - static inline bool isOnSegmentGC(double lat1, double lng1, double lat2, double lng2, double lat3, double lng3, double havTolerance) { - double havDist13 = MathUtil::havDistance(lat1, lat3, lng1 - lng3); - if (havDist13 <= havTolerance) { - return true; - } - double havDist23 = MathUtil::havDistance(lat2, lat3, lng2 - lng3); - if (havDist23 <= havTolerance) { - return true; - } - double sinBearing = PolyUtil::sinDeltaBearing(lat1, lng1, lat2, lng2, lat3, lng3); - double sinDist13 = MathUtil::sinFromHav(havDist13); - double havCrossTrack = MathUtil::havFromSin(sinDist13 * sinBearing); - if (havCrossTrack > havTolerance) { - return false; - } - double havDist12 = MathUtil::havDistance(lat1, lat2, lng1 - lng2); - double term = havDist12 + havCrossTrack * (1 - 2 * havDist12); - if (havDist13 > term || havDist23 > term) { - return false; - } - if (havDist12 < 0.74) { - return true; - } - double cosCrossTrack = 1 - 2 * havCrossTrack; - double havAlongTrack13 = (havDist13 - havCrossTrack) / cosCrossTrack; - double havAlongTrack23 = (havDist23 - havCrossTrack) / cosCrossTrack; - double sinSumAlongTrack = MathUtil::sinSumFromHav(havAlongTrack13, havAlongTrack23); - return sinSumAlongTrack > 0; // Compare with half-circle == PI using sign of sin(). - } + static inline bool isOnSegmentGC(double lat1, double lng1, double lat2, double lng2, double lat3, double lng3, double havTolerance) { + double havDist13 = MathUtil::havDistance(lat1, lat3, lng1 - lng3); + if (havDist13 <= havTolerance) { + return true; + } + double havDist23 = MathUtil::havDistance(lat2, lat3, lng2 - lng3); + if (havDist23 <= havTolerance) { + return true; + } + double sinBearing = PolyUtil::sinDeltaBearing(lat1, lng1, lat2, lng2, lat3, lng3); + double sinDist13 = MathUtil::sinFromHav(havDist13); + double havCrossTrack = MathUtil::havFromSin(sinDist13 * sinBearing); + if (havCrossTrack > havTolerance) { + return false; + } + double havDist12 = MathUtil::havDistance(lat1, lat2, lng1 - lng2); + double term = havDist12 + havCrossTrack * (1 - 2 * havDist12); + if (havDist13 > term || havDist23 > term) { + return false; + } + if (havDist12 < 0.74) { + return true; + } + double cosCrossTrack = 1 - 2 * havCrossTrack; + double havAlongTrack13 = (havDist13 - havCrossTrack) / cosCrossTrack; + double havAlongTrack23 = (havDist23 - havCrossTrack) / cosCrossTrack; + double sinSumAlongTrack = MathUtil::sinSumFromHav(havAlongTrack13, havAlongTrack23); + return sinSumAlongTrack > 0; // Compare with half-circle == PI using sign of sin(). + } }; #endif // GEOMETRY_LIBRARY_POLY_UTIL diff --git a/include/SphericalUtil.hpp b/include/SphericalUtil.hpp index faad65e..2d19ef6 100644 --- a/include/SphericalUtil.hpp +++ b/include/SphericalUtil.hpp @@ -20,243 +20,243 @@ class SphericalUtil { public: - /** - * Returns the heading from one LatLng to another LatLng. Headings are - * expressed in degrees clockwise from North within the range [-180,180). - * - * @return The heading in degrees clockwise from north. - */ - inline static double computeHeading(LatLng from, LatLng to) { - // http://williams.best.vwh.net/avform.htm#Crs - double fromLat = deg2rad(from.lat); - double fromLng = deg2rad(from.lng); - double toLat = deg2rad(to.lat); - double toLng = deg2rad(to.lng); - double dLng = toLng - fromLng; - double heading = atan2( - sin(dLng) * cos(toLat), - cos(fromLat) * sin(toLat) - sin(fromLat) * cos(toLat) * cos(dLng)); + /** + * Returns the heading from one LatLng to another LatLng. Headings are + * expressed in degrees clockwise from North within the range [-180,180). + * + * @return The heading in degrees clockwise from north. + */ + inline static double computeHeading(LatLng from, LatLng to) { + // http://williams.best.vwh.net/avform.htm#Crs + double fromLat = deg2rad(from.lat); + double fromLng = deg2rad(from.lng); + double toLat = deg2rad(to.lat); + double toLng = deg2rad(to.lng); + double dLng = toLng - fromLng; + double heading = atan2( + sin(dLng) * cos(toLat), + cos(fromLat) * sin(toLat) - sin(fromLat) * cos(toLat) * cos(dLng)); - return MathUtil::wrap(rad2deg(heading), -180, 180); - } + return MathUtil::wrap(rad2deg(heading), -180, 180); + } - /** - * Returns the LatLng resulting from moving a distance from an origin - * in the specified heading (expressed in degrees clockwise from north). - * - * @param from The LatLng from which to start. - * @param distance The distance to travel. - * @param heading The heading in degrees clockwise from north. - */ - inline static LatLng computeOffset(LatLng from, double distance, double heading) { - distance /= MathUtil::EARTH_RADIUS; - heading = deg2rad(heading); - // http://williams.best.vwh.net/avform.htm#LL - double fromLat = deg2rad(from.lat); - double fromLng = deg2rad(from.lng); - double cosDistance = cos(distance); - double sinDistance = sin(distance); - double sinFromLat = sin(fromLat); - double cosFromLat = cos(fromLat); - double sinLat = cosDistance * sinFromLat + sinDistance * cosFromLat * cos(heading); - double dLng = atan2( - sinDistance * cosFromLat * sin(heading), - cosDistance - sinFromLat * sinLat); + /** + * Returns the LatLng resulting from moving a distance from an origin + * in the specified heading (expressed in degrees clockwise from north). + * + * @param from The LatLng from which to start. + * @param distance The distance to travel. + * @param heading The heading in degrees clockwise from north. + */ + inline static LatLng computeOffset(LatLng from, double distance, double heading) { + distance /= MathUtil::EARTH_RADIUS; + heading = deg2rad(heading); + // http://williams.best.vwh.net/avform.htm#LL + double fromLat = deg2rad(from.lat); + double fromLng = deg2rad(from.lng); + double cosDistance = cos(distance); + double sinDistance = sin(distance); + double sinFromLat = sin(fromLat); + double cosFromLat = cos(fromLat); + double sinLat = cosDistance * sinFromLat + sinDistance * cosFromLat * cos(heading); + double dLng = atan2( + sinDistance * cosFromLat * sin(heading), + cosDistance - sinFromLat * sinLat); - return LatLng(rad2deg(asin(sinLat)), rad2deg(fromLng + dLng)); - } + return LatLng(rad2deg(asin(sinLat)), rad2deg(fromLng + dLng)); + } - /** - * Returns the location of origin when provided with a LatLng destination, - * meters travelled and original heading. Headings are expressed in degrees - * clockwise from North. This function returns null when no solution is - * available. - * - * @param to The destination LatLng. - * @param distance The distance travelled, in meters. - * @param heading The heading in degrees clockwise from north. - */ - inline static LatLng computeOffsetOrigin(LatLng to, double distance, double heading) { - heading = deg2rad(heading); - distance /= MathUtil::EARTH_RADIUS; - // http://lists.maptools.org/pipermail/proj/2008-October/003939.html - double n1 = cos(distance); - double n2 = sin(distance) * cos(heading); - double n3 = sin(distance) * sin(heading); - double n4 = sin(deg2rad(to.lat)); - // There are two solutions for b. b = n2 * n4 +/- sqrt(), one solution results - // in the latitude outside the [-90, 90] range. We first try one solution and - // back off to the other if we are outside that range. - double n12 = n1 * n1; - double discriminant = n2 * n2 * n12 + n12 * n12 - n12 * n4 * n4; - - // TODO: No real solution which would make sense in LatLng-space. - // if (discriminant < 0) return null; - - double b = n2 * n4 + sqrt(discriminant); - b /= n1 * n1 + n2 * n2; - double a = (n4 - n2 * b) / n1; - double fromLatRadians = atan2(a, b); - if (fromLatRadians < -M_PI / 2 || fromLatRadians > M_PI / 2) { - b = n2 * n4 - sqrt(discriminant); - b /= n1 * n1 + n2 * n2; - fromLatRadians = atan2(a, b); - } + /** + * Returns the location of origin when provided with a LatLng destination, + * meters travelled and original heading. Headings are expressed in degrees + * clockwise from North. This function returns null when no solution is + * available. + * + * @param to The destination LatLng. + * @param distance The distance travelled, in meters. + * @param heading The heading in degrees clockwise from north. + */ + inline static LatLng computeOffsetOrigin(LatLng to, double distance, double heading) { + heading = deg2rad(heading); + distance /= MathUtil::EARTH_RADIUS; + // http://lists.maptools.org/pipermail/proj/2008-October/003939.html + double n1 = cos(distance); + double n2 = sin(distance) * cos(heading); + double n3 = sin(distance) * sin(heading); + double n4 = sin(deg2rad(to.lat)); + // There are two solutions for b. b = n2 * n4 +/- sqrt(), one solution results + // in the latitude outside the [-90, 90] range. We first try one solution and + // back off to the other if we are outside that range. + double n12 = n1 * n1; + double discriminant = n2 * n2 * n12 + n12 * n12 - n12 * n4 * n4; + + // TODO: No real solution which would make sense in LatLng-space. + // if (discriminant < 0) return null; + + double b = n2 * n4 + sqrt(discriminant); + b /= n1 * n1 + n2 * n2; + double a = (n4 - n2 * b) / n1; + double fromLatRadians = atan2(a, b); + if (fromLatRadians < -M_PI / 2 || fromLatRadians > M_PI / 2) { + b = n2 * n4 - sqrt(discriminant); + b /= n1 * n1 + n2 * n2; + fromLatRadians = atan2(a, b); + } - // TODO: No solution which would make sense in LatLng-space. - // if (fromLatRadians < -M_PI / 2 || fromLatRadians > M_PI / 2) return null; + // TODO: No solution which would make sense in LatLng-space. + // if (fromLatRadians < -M_PI / 2 || fromLatRadians > M_PI / 2) return null; - double fromLngRadians = rad2deg(to.lng) - atan2(n3, n1 * cos(fromLatRadians) - n2 * sin(fromLatRadians)); - return LatLng(rad2deg(fromLatRadians), rad2deg(fromLngRadians)); - } + double fromLngRadians = rad2deg(to.lng) - atan2(n3, n1 * cos(fromLatRadians) - n2 * sin(fromLatRadians)); + return LatLng(rad2deg(fromLatRadians), rad2deg(fromLngRadians)); + } - /** - * Returns the LatLng which lies the given fraction of the way between the - * origin LatLng and the destination LatLng. - * - * @param from The LatLng from which to start. - * @param to The LatLng toward which to travel. - * @param fraction A fraction of the distance to travel. - * @return The interpolated LatLng. - */ - inline static LatLng interpolate(LatLng from, LatLng to, double fraction) { - // http://en.wikipedia.org/wiki/Slerp - double fromLat = deg2rad(from.lat); - double fromLng = deg2rad(from.lng); - double toLat = deg2rad(to.lat); - double toLng = deg2rad(to.lng); - double cosFromLat = cos(fromLat); - double cosToLat = cos(toLat); - // Computes Spherical interpolation coefficients. - double angle = SphericalUtil::computeAngleBetween(from, to); - double sinAngle = sin(angle); - if (sinAngle < 1e-6) { - return from; - } - double a = sin((1 - fraction) * angle) / sinAngle; - double b = sin(fraction * angle) / sinAngle; - // Converts from polar to vector and interpolate. - double x = a * cosFromLat * cos(fromLng) + b * cosToLat * cos(toLng); - double y = a * cosFromLat * sin(fromLng) + b * cosToLat * sin(toLng); - double z = a * sin(fromLat) + b * sin(toLat); - // Converts interpolated vector back to polar. - double lat = atan2(z, sqrt(x * x + y * y)); - double lng = atan2(y, x); - return LatLng(rad2deg(lat), rad2deg(lng)); - } + /** + * Returns the LatLng which lies the given fraction of the way between the + * origin LatLng and the destination LatLng. + * + * @param from The LatLng from which to start. + * @param to The LatLng toward which to travel. + * @param fraction A fraction of the distance to travel. + * @return The interpolated LatLng. + */ + inline static LatLng interpolate(LatLng from, LatLng to, double fraction) { + // http://en.wikipedia.org/wiki/Slerp + double fromLat = deg2rad(from.lat); + double fromLng = deg2rad(from.lng); + double toLat = deg2rad(to.lat); + double toLng = deg2rad(to.lng); + double cosFromLat = cos(fromLat); + double cosToLat = cos(toLat); + // Computes Spherical interpolation coefficients. + double angle = SphericalUtil::computeAngleBetween(from, to); + double sinAngle = sin(angle); + if (sinAngle < 1e-6) { + return from; + } + double a = sin((1 - fraction) * angle) / sinAngle; + double b = sin(fraction * angle) / sinAngle; + // Converts from polar to vector and interpolate. + double x = a * cosFromLat * cos(fromLng) + b * cosToLat * cos(toLng); + double y = a * cosFromLat * sin(fromLng) + b * cosToLat * sin(toLng); + double z = a * sin(fromLat) + b * sin(toLat); + // Converts interpolated vector back to polar. + double lat = atan2(z, sqrt(x * x + y * y)); + double lng = atan2(y, x); + return LatLng(rad2deg(lat), rad2deg(lng)); + } - /** - * Returns the angle between two LatLngs, in radians. This is the same as the distance - * on the unit sphere. - */ - inline static double computeAngleBetween(LatLng from, LatLng to) { - return SphericalUtil::distanceRadians(deg2rad(from.lat), deg2rad(from.lng), deg2rad(to.lat), deg2rad(to.lng)); - } + /** + * Returns the angle between two LatLngs, in radians. This is the same as the distance + * on the unit sphere. + */ + inline static double computeAngleBetween(LatLng from, LatLng to) { + return SphericalUtil::distanceRadians(deg2rad(from.lat), deg2rad(from.lng), deg2rad(to.lat), deg2rad(to.lng)); + } - /** - * Returns the distance between two LatLngs, in meters. - */ - inline static double computeDistanceBetween(LatLng from, LatLng to) { - return SphericalUtil::computeAngleBetween(from, to) * MathUtil::EARTH_RADIUS; - } + /** + * Returns the distance between two LatLngs, in meters. + */ + inline static double computeDistanceBetween(LatLng from, LatLng to) { + return SphericalUtil::computeAngleBetween(from, to) * MathUtil::EARTH_RADIUS; + } - /** - * Returns the length of the given path, in meters, on Earth. - */ - template - inline static double computeLength(LatLngList path) { - if (path.size() < 2U) { - return 0; - } - double length = 0; - LatLng prev = path[0]; - double prevLat = deg2rad(prev.lat); - double prevLng = deg2rad(prev.lng); - for (auto point : path) { - double lat = deg2rad(point.lat); - double lng = deg2rad(point.lng); - length += SphericalUtil::distanceRadians(prevLat, prevLng, lat, lng); - prevLat = lat; - prevLng = lng; - } - return length * MathUtil::EARTH_RADIUS; - } + /** + * Returns the length of the given path, in meters, on Earth. + */ + template + inline static double computeLength(LatLngList path) { + if (path.size() < 2U) { + return 0; + } + double length = 0; + LatLng prev = path[0]; + double prevLat = deg2rad(prev.lat); + double prevLng = deg2rad(prev.lng); + for (auto point : path) { + double lat = deg2rad(point.lat); + double lng = deg2rad(point.lng); + length += SphericalUtil::distanceRadians(prevLat, prevLng, lat, lng); + prevLat = lat; + prevLng = lng; + } + return length * MathUtil::EARTH_RADIUS; + } - /** - * Returns the area of a closed path on Earth. - * - * @param path A closed path. - * @return The path's area in square meters. - */ - template - inline static double computeArea(LatLngList path) { - return abs(SphericalUtil::computeSignedArea(path)); - } + /** + * Returns the area of a closed path on Earth. + * + * @param path A closed path. + * @return The path's area in square meters. + */ + template + inline static double computeArea(LatLngList path) { + return abs(SphericalUtil::computeSignedArea(path)); + } - /** - * Returns the signed area of a closed path on Earth. The sign of the area may be used to - * determine the orientation of the path. - * "inside" is the surface that does not contain the South Pole. - * - * @param path A closed path. - * @return The loop's area in square meters. - */ - template - inline static double computeSignedArea(LatLngList path) { - return SphericalUtil::computeSignedAreaP(path, MathUtil::EARTH_RADIUS); - } + /** + * Returns the signed area of a closed path on Earth. The sign of the area may be used to + * determine the orientation of the path. + * "inside" is the surface that does not contain the South Pole. + * + * @param path A closed path. + * @return The loop's area in square meters. + */ + template + inline static double computeSignedArea(LatLngList path) { + return SphericalUtil::computeSignedAreaP(path, MathUtil::EARTH_RADIUS); + } private: - /** - * Returns distance on the unit sphere; the arguments are in radians. - */ - inline static double distanceRadians(double lat1, double lng1, double lat2, double lng2) { - return MathUtil::arcHav(MathUtil::havDistance(lat1, lat2, lng1 - lng2)); - } + /** + * Returns distance on the unit sphere; the arguments are in radians. + */ + inline static double distanceRadians(double lat1, double lng1, double lat2, double lng2) { + return MathUtil::arcHav(MathUtil::havDistance(lat1, lat2, lng1 - lng2)); + } - /** - * Returns the signed area of a closed path on a sphere of given radius. - * The computed area uses the same units as the radius squared. - * Used by SphericalUtilTest. - */ - template - inline static double computeSignedAreaP(LatLngList path, double radius) { - size_t size = path.size(); - if (size < 3U) { return 0; } - double total = 0; - LatLng prev = path[size - 1]; - double prevTanLat = tan((M_PI / 2 - deg2rad(prev.lat)) / 2); - double prevLng = deg2rad(prev.lng); - // For each edge, accumulate the signed area of the triangle formed by the North Pole - // and that edge ("polar triangle"). - for (auto point : path) { - double tanLat = tan((M_PI / 2 - deg2rad(point.lat)) / 2); - double lng = deg2rad(point.lng); - total += SphericalUtil::polarTriangleArea(tanLat, lng, prevTanLat, prevLng); - prevTanLat = tanLat; - prevLng = lng; - } - return total * (radius * radius); - } + /** + * Returns the signed area of a closed path on a sphere of given radius. + * The computed area uses the same units as the radius squared. + * Used by SphericalUtilTest. + */ + template + inline static double computeSignedAreaP(LatLngList path, double radius) { + size_t size = path.size(); + if (size < 3U) { return 0; } + double total = 0; + LatLng prev = path[size - 1]; + double prevTanLat = tan((M_PI / 2 - deg2rad(prev.lat)) / 2); + double prevLng = deg2rad(prev.lng); + // For each edge, accumulate the signed area of the triangle formed by the North Pole + // and that edge ("polar triangle"). + for (auto point : path) { + double tanLat = tan((M_PI / 2 - deg2rad(point.lat)) / 2); + double lng = deg2rad(point.lng); + total += SphericalUtil::polarTriangleArea(tanLat, lng, prevTanLat, prevLng); + prevTanLat = tanLat; + prevLng = lng; + } + return total * (radius * radius); + } - /** - * Returns the signed area of a triangle which has North Pole as a vertex. - * Formula derived from "Area of a spherical triangle given two edges and the included angle" - * as per "Spherical Trigonometry" by Todhunter, page 71, section 103, point 2. - * See http://books.google.com/books?id=3uBHAAAAIAAJ&pg=PA71 - * The arguments named "tan" are tan((pi/2 - latitude)/2). - */ - inline static double polarTriangleArea(double tan1, double lng1, double tan2, double lng2) { - double deltaLng = lng1 - lng2; - double t = tan1 * tan2; - return 2 * atan2(t * sin(deltaLng), 1 + t * cos(deltaLng)); - } + /** + * Returns the signed area of a triangle which has North Pole as a vertex. + * Formula derived from "Area of a spherical triangle given two edges and the included angle" + * as per "Spherical Trigonometry" by Todhunter, page 71, section 103, point 2. + * See http://books.google.com/books?id=3uBHAAAAIAAJ&pg=PA71 + * The arguments named "tan" are tan((pi/2 - latitude)/2). + */ + inline static double polarTriangleArea(double tan1, double lng1, double tan2, double lng2) { + double deltaLng = lng1 - lng2; + double t = tan1 * tan2; + return 2 * atan2(t * sin(deltaLng), 1 + t * cos(deltaLng)); + } }; #endif // GEOMETRY_LIBRARY_SPHERICAL_UTIL diff --git a/tests/SphericalUtil/computeAngleBetween.hpp b/tests/SphericalUtil/computeAngleBetween.hpp index 20fa58e..dca6ec2 100644 --- a/tests/SphericalUtil/computeAngleBetween.hpp +++ b/tests/SphericalUtil/computeAngleBetween.hpp @@ -11,31 +11,31 @@ TEST(SphericalUtil, computeAngleBetween) { LatLng back = { 0.0, -180.0 }; LatLng left = { 0.0, -90.0 }; - EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, up), 0, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, down), 0, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(left, left), 0, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(right, right), 0, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(front, front), 0, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, back), 0, 1e-6); - - // Adjacent vertices - EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, front), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, right), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, back), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, left), M_PI / 2, 1e-6); - - EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, front), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, right), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, back), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, left), M_PI / 2, 1e-6); - - EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, up), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, right), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, down), M_PI / 2, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, left), M_PI / 2, 1e-6); - - // Opposite vertices - EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, down), M_PI, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(front, back), M_PI, 1e-6); - EXPECT_NEAR(SphericalUtil::computeAngleBetween(left, right), M_PI, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, up), 0, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, down), 0, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(left, left), 0, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(right, right), 0, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(front, front), 0, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, back), 0, 1e-6); + + // Adjacent vertices + EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, front), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, right), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, back), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, left), M_PI / 2, 1e-6); + + EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, front), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, right), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, back), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(down, left), M_PI / 2, 1e-6); + + EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, up), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, right), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, down), M_PI / 2, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(back, left), M_PI / 2, 1e-6); + + // Opposite vertices + EXPECT_NEAR(SphericalUtil::computeAngleBetween(up, down), M_PI, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(front, back), M_PI, 1e-6); + EXPECT_NEAR(SphericalUtil::computeAngleBetween(left, right), M_PI, 1e-6); } diff --git a/tests/SphericalUtil/computeArea.hpp b/tests/SphericalUtil/computeArea.hpp index 1120398..1768bd1 100644 --- a/tests/SphericalUtil/computeArea.hpp +++ b/tests/SphericalUtil/computeArea.hpp @@ -4,12 +4,10 @@ TEST(SphericalUtil, computeArea) { - LatLng up = { 90.0, 0.0 }; - LatLng down = {-90.0, 0.0 }; - LatLng front = { 0.0, 0.0 }; - LatLng right = { 0.0, 90.0 }; - LatLng back = { 0.0, -180.0 }; - LatLng left = { 0.0, -90.0 }; + LatLng up = { 90.0, 0.0 }; + LatLng down = {-90.0, 0.0 }; + LatLng front = { 0.0, 0.0 }; + LatLng right = { 0.0, 90.0 }; std::vector first = { right, up, front, down, right }; EXPECT_NEAR(SphericalUtil::computeArea(first), M_PI * MathUtil::EARTH_RADIUS * MathUtil::EARTH_RADIUS, .4); diff --git a/tests/SphericalUtil/computeDistanceBetween.hpp b/tests/SphericalUtil/computeDistanceBetween.hpp index 85a24ba..e514c46 100644 --- a/tests/SphericalUtil/computeDistanceBetween.hpp +++ b/tests/SphericalUtil/computeDistanceBetween.hpp @@ -4,8 +4,8 @@ TEST(SphericalUtil, computeDistanceBetween) { - LatLng up = { 90.0, 0.0}; - LatLng down = {-90.0, 0.0}; + LatLng up = { 90.0, 0.0}; + LatLng down = {-90.0, 0.0}; - EXPECT_NEAR(SphericalUtil::computeDistanceBetween(up, down), M_PI * MathUtil::EARTH_RADIUS, 1e-6); + EXPECT_NEAR(SphericalUtil::computeDistanceBetween(up, down), M_PI * MathUtil::EARTH_RADIUS, 1e-6); } diff --git a/tests/SphericalUtil/computeHeading.hpp b/tests/SphericalUtil/computeHeading.hpp index 481f80b..7b217b2 100644 --- a/tests/SphericalUtil/computeHeading.hpp +++ b/tests/SphericalUtil/computeHeading.hpp @@ -11,24 +11,24 @@ TEST(SphericalUtil, computeHeading) { LatLng back = { 0.0, -180.0 }; LatLng left = { 0.0, -90.0 }; - // Opposing vertices for which there is a result - EXPECT_NEAR(SphericalUtil::computeHeading(up, down), -180, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(down, up), 0, 1e-6); - - // Adjacent vertices for which there is a result - EXPECT_NEAR(SphericalUtil::computeHeading(front, up), 0, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(right, up), 0, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(back, up), 0, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(down, up), 0, 1e-6); - - EXPECT_NEAR(SphericalUtil::computeHeading(front, down), -180, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(right, down), -180, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(back, down), -180, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(left, down), -180, 1e-6); - - EXPECT_NEAR(SphericalUtil::computeHeading(right, front), -90, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(left, front), 90, 1e-6); - - EXPECT_NEAR(SphericalUtil::computeHeading(front, right), 90, 1e-6); - EXPECT_NEAR(SphericalUtil::computeHeading(back, right), -90, 1e-6); + // Opposing vertices for which there is a result + EXPECT_NEAR(SphericalUtil::computeHeading(up, down), -180, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(down, up), 0, 1e-6); + + // Adjacent vertices for which there is a result + EXPECT_NEAR(SphericalUtil::computeHeading(front, up), 0, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(right, up), 0, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(back, up), 0, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(down, up), 0, 1e-6); + + EXPECT_NEAR(SphericalUtil::computeHeading(front, down), -180, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(right, down), -180, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(back, down), -180, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(left, down), -180, 1e-6); + + EXPECT_NEAR(SphericalUtil::computeHeading(right, front), -90, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(left, front), 90, 1e-6); + + EXPECT_NEAR(SphericalUtil::computeHeading(front, right), 90, 1e-6); + EXPECT_NEAR(SphericalUtil::computeHeading(back, right), -90, 1e-6); } diff --git a/tests/SphericalUtil/computeOffset.hpp b/tests/SphericalUtil/computeOffset.hpp index aa16f4c..1360249 100644 --- a/tests/SphericalUtil/computeOffset.hpp +++ b/tests/SphericalUtil/computeOffset.hpp @@ -5,9 +5,9 @@ #define EXPECT_NEAR_LatLan(actual, expected) \ EXPECT_NEAR(actual.lat, expected.lat, 1e-6); // Issue #2 - // Account for the convergence of longitude lines at the poles - // double cosLat = cos(deg2rad(actual.lat)); - // EXPECT_NEAR(cosLat * actual.lng, cosLat * expected.lng, 1e-6); + // Account for the convergence of longitude lines at the poles + // double cosLat = cos(deg2rad(actual.lat)); + // EXPECT_NEAR(cosLat * actual.lng, cosLat * expected.lng, 1e-6); TEST(SphericalUtil, computeOffset) { LatLng up = { 90.0, 0.0 }; @@ -17,22 +17,22 @@ TEST(SphericalUtil, computeOffset) { LatLng back = { 0.0, -180.0 }; LatLng left = { 0.0, -90.0 }; - EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffset(front, 0, 0)); - EXPECT_NEAR_LatLan(up, SphericalUtil::computeOffset(front, M_PI * MathUtil::EARTH_RADIUS / 2, 0)); + EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffset(front, 0, 0)); + EXPECT_NEAR_LatLan(up, SphericalUtil::computeOffset(front, M_PI * MathUtil::EARTH_RADIUS / 2, 0)); EXPECT_NEAR_LatLan(down, SphericalUtil::computeOffset(front, M_PI * MathUtil::EARTH_RADIUS / 2, 180)); - EXPECT_NEAR_LatLan(left, SphericalUtil::computeOffset(front, M_PI * MathUtil::EARTH_RADIUS / 2, -90)); + EXPECT_NEAR_LatLan(left, SphericalUtil::computeOffset(front, M_PI * MathUtil::EARTH_RADIUS / 2, -90)); EXPECT_NEAR_LatLan(right, SphericalUtil::computeOffset(front, M_PI * MathUtil::EARTH_RADIUS / 2, 90)); EXPECT_NEAR_LatLan(back, SphericalUtil::computeOffset(front, M_PI * MathUtil::EARTH_RADIUS, 0)); EXPECT_NEAR_LatLan(back, SphericalUtil::computeOffset(front, M_PI * MathUtil::EARTH_RADIUS, 90)); - // From left - EXPECT_NEAR_LatLan(left, SphericalUtil::computeOffset(left, 0, 0)); - EXPECT_NEAR_LatLan(up, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS / 2, 0)); - EXPECT_NEAR_LatLan(down, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS / 2, 180)); - EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS / 2, 90)); - EXPECT_NEAR_LatLan(back, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS / 2, -90)); - EXPECT_NEAR_LatLan(right, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS, 0)); - EXPECT_NEAR_LatLan(right, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS, 90)); + // From left + EXPECT_NEAR_LatLan(left, SphericalUtil::computeOffset(left, 0, 0)); + EXPECT_NEAR_LatLan(up, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS / 2, 0)); + EXPECT_NEAR_LatLan(down, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS / 2, 180)); + EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS / 2, 90)); + EXPECT_NEAR_LatLan(back, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS / 2, -90)); + EXPECT_NEAR_LatLan(right, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS, 0)); + EXPECT_NEAR_LatLan(right, SphericalUtil::computeOffset(left, M_PI * MathUtil::EARTH_RADIUS, 90)); - // NOTE: Heading is undefined at the poles, so we do not test from up/down. + // NOTE: Heading is undefined at the poles, so we do not test from up/down. } diff --git a/tests/SphericalUtil/computeOffsetOrigin.hpp b/tests/SphericalUtil/computeOffsetOrigin.hpp index 29d17fa..4006928 100644 --- a/tests/SphericalUtil/computeOffsetOrigin.hpp +++ b/tests/SphericalUtil/computeOffsetOrigin.hpp @@ -13,19 +13,19 @@ TEST(SphericalUtil, computeOffsetOrigin) { LatLng front = { 0.0, 0.0 }; - EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(front, 0, 0)); + EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(front, 0, 0)); - EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(LatLng( 0, 45), M_PI * MathUtil::EARTH_RADIUS / 4, 90)); - EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(LatLng( 0, -45), M_PI * MathUtil::EARTH_RADIUS / 4, -90)); - EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(LatLng( 45, 0), M_PI * MathUtil::EARTH_RADIUS / 4, 0)); - EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(LatLng(-45, 0), M_PI * MathUtil::EARTH_RADIUS / 4, 180)); + EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(LatLng( 0, 45), M_PI * MathUtil::EARTH_RADIUS / 4, 90)); + EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(LatLng( 0, -45), M_PI * MathUtil::EARTH_RADIUS / 4, -90)); + EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(LatLng( 45, 0), M_PI * MathUtil::EARTH_RADIUS / 4, 0)); + EXPECT_NEAR_LatLan(front, SphericalUtil::computeOffsetOrigin(LatLng(-45, 0), M_PI * MathUtil::EARTH_RADIUS / 4, 180)); // Issue #3 - // Situations with no solution, should return null. - // - // First 'over' the pole. - // EXPECT_NULL(SphericalUtil::computeOffsetOrigin(LatLng(80, 0), M_PI * MathUtil::EARTH_RADIUS / 4, 180)); - - // Second a distance that doesn't fit on the earth. - // EXPECT_NULL(SphericalUtil::computeOffsetOrigin(LatLng(80, 0), M_PI * MathUtil::EARTH_RADIUS / 4, 90)); + // Situations with no solution, should return null. + // + // First 'over' the pole. + // EXPECT_NULL(SphericalUtil::computeOffsetOrigin(LatLng(80, 0), M_PI * MathUtil::EARTH_RADIUS / 4, 180)); + + // Second a distance that doesn't fit on the earth. + // EXPECT_NULL(SphericalUtil::computeOffsetOrigin(LatLng(80, 0), M_PI * MathUtil::EARTH_RADIUS / 4, 90)); } diff --git a/tests/SphericalUtil/interpolate.hpp b/tests/SphericalUtil/interpolate.hpp index 2b00888..747ad1a 100644 --- a/tests/SphericalUtil/interpolate.hpp +++ b/tests/SphericalUtil/interpolate.hpp @@ -15,7 +15,6 @@ TEST(SphericalUtil, interpolate) { LatLng up = { 90.0, 0.0 }; LatLng down = {-90.0, 0.0 }; LatLng front = { 0.0, 0.0 }; - LatLng right = { 0.0, 90.0 }; LatLng back = { 0.0, -180.0 }; LatLng left = { 0.0, -90.0 };