From 493b13364f8b89f08d805ac783ddf1328bc2c880 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Wed, 21 May 2014 12:33:54 +0200 Subject: [PATCH] move geographical distance computation to floats --- DataStructures/Coordinate.cpp | 202 ++++++++-------------------- DataStructures/StaticRTree.h | 55 ++++---- Include/osrm/Coordinate.h | 25 ++-- Util/MercatorUtil.h | 10 +- features/testbot/projection.feature | 2 +- 5 files changed, 103 insertions(+), 191 deletions(-) diff --git a/DataStructures/Coordinate.cpp b/DataStructures/Coordinate.cpp index 6938aa723..3e99f8ec8 100644 --- a/DataStructures/Coordinate.cpp +++ b/DataStructures/Coordinate.cpp @@ -148,20 +148,20 @@ float FixedPointCoordinate::ApproximateEuclideanDistance(const int lat1, return sqrt(x * x + y * y) * earth_radius; } -double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &point, +float FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &point, const FixedPointCoordinate &segA, const FixedPointCoordinate &segB) { - const double x = lat2y(point.lat / COORDINATE_PRECISION); - const double y = point.lon / COORDINATE_PRECISION; - const double a = lat2y(segA.lat / COORDINATE_PRECISION); - const double b = segA.lon / COORDINATE_PRECISION; - const double c = lat2y(segB.lat / COORDINATE_PRECISION); - const double d = segB.lon / COORDINATE_PRECISION; - double p, q, nY; - if (std::abs(a - c) > std::numeric_limits::epsilon()) + const float x = lat2y(point.lat / COORDINATE_PRECISION); + const float y = point.lon / COORDINATE_PRECISION; + const float a = lat2y(segA.lat / COORDINATE_PRECISION); + const float b = segA.lon / COORDINATE_PRECISION; + const float c = lat2y(segB.lat / COORDINATE_PRECISION); + const float d = segB.lon / COORDINATE_PRECISION; + float p, q, nY; + if (std::abs(a - c) > std::numeric_limits::epsilon()) { - const double m = (d - b) / (c - a); // slope + const float m = (d - b) / (c - a); // slope // Projection of (x,y) on line joining (a,b) and (c,d) p = ((x + (m * y)) + (m * m * a - m * b)) / (1. + m * m); q = b + m * (p - a); @@ -179,16 +179,16 @@ double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordi nY = 0.; } - double r = (p - nY * a) / c; + float r = (p - nY * a) / c; if (std::isnan(r)) { r = ((segB.lat == point.lat) && (segB.lon == point.lon)) ? 1. : 0.; } - else if (std::abs(r) <= std::numeric_limits::epsilon()) + else if (std::abs(r) <= std::numeric_limits::epsilon()) { r = 0.; } - else if (std::abs(r - 1.) <= std::numeric_limits::epsilon()) + else if (std::abs(r - 1.) <= std::numeric_limits::epsilon()) { r = 1.; } @@ -210,30 +210,30 @@ double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordi nearest_location.lon = q * COORDINATE_PRECISION; } BOOST_ASSERT(nearest_location.isValid()); - const double approximated_distance = + const float approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance(point, nearest_location); BOOST_ASSERT(0. <= approximated_distance); return approximated_distance; } -double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &coord_a, +float FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &coord_a, const FixedPointCoordinate &coord_b, const FixedPointCoordinate &query_location, FixedPointCoordinate &nearest_location, - double &r) + float &r) { BOOST_ASSERT(query_location.isValid()); - const double x = lat2y(query_location.lat / COORDINATE_PRECISION); - const double y = query_location.lon / COORDINATE_PRECISION; - const double a = lat2y(coord_a.lat / COORDINATE_PRECISION); - const double b = coord_a.lon / COORDINATE_PRECISION; - const double c = lat2y(coord_b.lat / COORDINATE_PRECISION); - const double d = coord_b.lon / COORDINATE_PRECISION; - double p, q /*,mX*/, nY; - if (std::abs(a - c) > std::numeric_limits::epsilon()) + const float x = lat2y(query_location.lat / COORDINATE_PRECISION); + const float y = query_location.lon / COORDINATE_PRECISION; + const float a = lat2y(coord_a.lat / COORDINATE_PRECISION); + const float b = coord_a.lon / COORDINATE_PRECISION; + const float c = lat2y(coord_b.lat / COORDINATE_PRECISION); + const float d = coord_b.lon / COORDINATE_PRECISION; + float p, q /*,mX*/, nY; + if (std::abs(a - c) > std::numeric_limits::epsilon()) { - const double m = (d - b) / (c - a); // slope + const float m = (d - b) / (c - a); // slope // Projection of (x,y) on line joining (a,b) and (c,d) p = ((x + (m * y)) + (m * m * a - m * b)) / (1. + m * m); q = b + m * (p - a); @@ -258,11 +258,11 @@ double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordi { r = ((coord_b.lat == query_location.lat) && (coord_b.lon == query_location.lon)) ? 1. : 0.; } - else if (std::abs(r) <= std::numeric_limits::epsilon()) + else if (std::abs(r) <= std::numeric_limits::epsilon()) { r = 0.; } - else if (std::abs(r - 1.) <= std::numeric_limits::epsilon()) + else if (std::abs(r - 1.) <= std::numeric_limits::epsilon()) { r = 1.; } @@ -284,8 +284,8 @@ double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordi BOOST_ASSERT(nearest_location.isValid()); // TODO: Replace with euclidean approximation when k-NN search is done - // const double approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance( - const double approximated_distance = + // const float approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance( + const float approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance(query_location, nearest_location); BOOST_ASSERT(0. <= approximated_distance); return approximated_distance; @@ -328,145 +328,53 @@ void FixedPointCoordinate::Output(std::ostream &out) const out << "(" << lat / COORDINATE_PRECISION << "," << lon / COORDINATE_PRECISION << ")"; } -double FixedPointCoordinate::GetBearing(const FixedPointCoordinate &A, const FixedPointCoordinate &B) +float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &A, const FixedPointCoordinate &B) { - double delta_long = DegreeToRadian(B.lon / COORDINATE_PRECISION - A.lon / COORDINATE_PRECISION); - - const double lat1 = DegreeToRadian(A.lat / COORDINATE_PRECISION); - const double lat2 = DegreeToRadian(B.lat / COORDINATE_PRECISION); - - const double y = sin(delta_long) * cos(lat2); - const double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_long); - double result = RadianToDegree(atan2_lookup(y, x)); - while (result < 0.) + const float delta_long = DegreeToRadian(B.lon / COORDINATE_PRECISION - A.lon / COORDINATE_PRECISION); + const float lat1 = DegreeToRadian(A.lat / COORDINATE_PRECISION); + const float lat2 = DegreeToRadian(B.lat / COORDINATE_PRECISION); + const float y = sin(delta_long) * cos(lat2); + const float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_long); + float result = RadianToDegree(std::atan2(y, x)); + while (result < 0.f) { - result += 360.; + result += 360.f; } - while (result >= 360.) + while (result >= 360.f) { - result -= 360.; + result -= 360.f; } return result; } -double FixedPointCoordinate::GetBearing(const FixedPointCoordinate &other) const +float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &other) const { - double delta_long = DegreeToRadian(lon / COORDINATE_PRECISION - other.lon / COORDINATE_PRECISION); + const float delta_long = DegreeToRadian(lon / COORDINATE_PRECISION - other.lon / COORDINATE_PRECISION); + const float lat1 = DegreeToRadian(other.lat / COORDINATE_PRECISION); + const float lat2 = DegreeToRadian(lat / COORDINATE_PRECISION); + const float y = std::sin(delta_long) * std::cos(lat2); + const float x = std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(delta_long); + float result = RadianToDegree(std::atan2(y, x)); - const double lat1 = DegreeToRadian(other.lat / COORDINATE_PRECISION); - const double lat2 = DegreeToRadian(lat / COORDINATE_PRECISION); - - const double y = std::sin(delta_long) * std::cos(lat2); - const double x = std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(delta_long); - double result = RadianToDegree(atan2_lookup(y, x)); - - while (result < 0.) + while (result < 0.f) { - result += 360.; + result += 360.f; } - while (result >= 360.) + while (result >= 360.f) { - result -= 360.; + result -= 360.f; } return result; } -double FixedPointCoordinate::DegreeToRadian(const double degree) +float FixedPointCoordinate::DegreeToRadian(const float degree) { - return degree * (M_PI / 180.); + return degree * (M_PI / 180.f); } -double FixedPointCoordinate::RadianToDegree(const double radian) +float FixedPointCoordinate::RadianToDegree(const float radian) { - return radian * (180. / M_PI); + return radian * (180.f / M_PI); } - -// double PointSegmentDistanceSquared( double px, double py, -// double p1x, double p1y, -// double p2x, double p2y, -// double& t, -// double& qx, double& qy) -// { -// static const double kMinSegmentLenSquared = 0.00000001; // adjust to suit. If you use float, you'll probably want something like 0.000001f -// static const double kEpsilon = 1.0E-14; // adjust to suit. If you use floats, you'll probably want something like 1E-7f -// double dx = p2x - p1x; -// double dy = p2y - p1y; -// double dp1x = px - p1x; -// double dp1y = py - p1y; -// const double segLenSquared = (dx * dx) + (dy * dy); -// if (segLenSquared >= -kMinSegmentLenSquared && segLenSquared <= kMinSegmentLenSquared) -// { -// // segment is a point. -// qx = p1x; -// qy = p1y; -// t = 0.0; -// return ((dp1x * dp1x) + (dp1y * dp1y)); -// } -// else -// { -// // Project a line from p to the segment [p1,p2]. By considering the line -// // extending the segment, parameterized as p1 + (t * (p2 - p1)), -// // we find projection of point p onto the line. -// // It falls where t = [(p - p1) . (p2 - p1)] / |p2 - p1|^2 -// t = ((dp1x * dx) + (dp1y * dy)) / segLenSquared; -// if (t < kEpsilon) -// { -// // intersects at or to the "left" of first segment vertex (p1x, p1y). If t is approximately 0.0, then -// // intersection is at p1. If t is less than that, then there is no intersection (i.e. p is not within -// // the 'bounds' of the segment) -// if (t > -kEpsilon) -// { -// // intersects at 1st segment vertex -// t = 0.0; -// } -// // set our 'intersection' point to p1. -// qx = p1x; -// qy = p1y; -// // Note: If you wanted the ACTUAL intersection point of where the projected lines would intersect if -// // we were doing PointLineDistanceSquared, then qx would be (p1x + (t * dx)) and qy would be (p1y + (t * dy)). -// } -// else if (t > (1.0 - kEpsilon)) -// { -// // intersects at or to the "right" of second segment vertex (p2x, p2y). If t is approximately 1.0, then -// // intersection is at p2. If t is greater than that, then there is no intersection (i.e. p is not within -// // the 'bounds' of the segment) -// if (t < (1.0 + kEpsilon)) -// { -// // intersects at 2nd segment vertex -// t = 1.0; -// } -// // set our 'intersection' point to p2. -// qx = p2x; -// qy = p2y; -// // Note: If you wanted the ACTUAL intersection point of where the projected lines would intersect if -// // we were doing PointLineDistanceSquared, then qx would be (p1x + (t * dx)) and qy would be (p1y + (t * dy)). -// } -// else -// { -// // The projection of the point to the point on the segment that is perpendicular succeeded and the point -// // is 'within' the bounds of the segment. Set the intersection point as that projected point. -// qx = p1x + (t * dx); -// qy = p1y + (t * dy); -// } -// // return the squared distance from p to the intersection point. Note that we return the squared distance -// // as an optimization because many times you just need to compare relative distances and the squared values -// // works fine for that. If you want the ACTUAL distance, just take the square root of this value. -// double dpqx = px - qx; -// double dpqy = py - qy; -// return ((dpqx * dpqx) + (dpqy * dpqy)); -// } -// } - -// public float DistanceOfPointToLine2(PointF p1, PointF p2, PointF p) -// { -// // (y1-y2)x + (x2-x1)y + (x1y2-x2y1) -// //d(P,L) = -------------------------------- -// // sqrt( (x2-x1)pow2 + (y2-y1)pow2 ) - -// double ch = (p1.Y - p2.Y) * p.X + (p2.X - p1.X) * p.Y + (p1.X * p2.Y - p2.X * p1.Y); -// double del = Math.Sqrt(Math.Pow(p2.X - p1.X, 2) + Math.Pow(p2.Y - p1.Y, 2)); -// double d = ch / del; -// return (float)d; -// } diff --git a/DataStructures/StaticRTree.h b/DataStructures/StaticRTree.h index b3135f0c1..a1f11908d 100644 --- a/DataStructures/StaticRTree.h +++ b/DataStructures/StaticRTree.h @@ -128,7 +128,7 @@ class StaticRTree Contains(lower_left)); } - inline double GetMinDist(const FixedPointCoordinate &location) const + inline float GetMinDist(const FixedPointCoordinate &location) const { bool is_contained = Contains(location); if (is_contained) @@ -136,7 +136,7 @@ class StaticRTree return 0.; } - double min_dist = std::numeric_limits::max(); + float min_dist = std::numeric_limits::max(); min_dist = std::min(min_dist, FixedPointCoordinate::ApproximateEuclideanDistance( location.lat, location.lon, max_lat, min_lon)); @@ -154,7 +154,7 @@ class StaticRTree inline float GetMinMaxDist(const FixedPointCoordinate &location) const { - float min_max_dist = std::numeric_limits::max(); + float min_max_dist = std::numeric_limits::max(); // Get minmax distance to each of the four sides FixedPointCoordinate upper_left(max_lat, min_lon); FixedPointCoordinate upper_right(max_lat, max_lon); @@ -240,13 +240,13 @@ class StaticRTree struct QueryCandidate { - explicit QueryCandidate(const uint32_t n_id, const double dist) + explicit QueryCandidate(const uint32_t n_id, const float dist) : node_id(n_id), min_dist(dist) { } - QueryCandidate() : node_id(UINT_MAX), min_dist(std::numeric_limits::max()) {} + QueryCandidate() : node_id(UINT_MAX), min_dist(std::numeric_limits::max()) {} uint32_t node_id; - double min_dist; + float min_dist; inline bool operator<(const QueryCandidate &other) const { return min_dist < other.min_dist; @@ -490,13 +490,13 @@ class StaticRTree { bool ignore_tiny_components = (zoom_level <= 14); DataT nearest_edge; - double min_dist = std::numeric_limits::max(); - double min_max_dist = std::numeric_limits::max(); + float min_dist = std::numeric_limits::max(); + float min_max_dist = std::numeric_limits::max(); bool found_a_nearest_edge = false; // initialize queue with root element std::priority_queue traversal_queue; - double current_min_dist = + float current_min_dist = m_search_tree[0].minimum_bounding_rectangle.GetMinDist(input_coordinate); traversal_queue.emplace(0, current_min_dist); @@ -522,7 +522,7 @@ class StaticRTree continue; } - double current_minimum_distance = + float current_minimum_distance = FixedPointCoordinate::ApproximateEuclideanDistance( input_coordinate.lat, input_coordinate.lon, @@ -563,9 +563,9 @@ class StaticRTree const TreeNode &child_tree_node = m_search_tree[child_id]; const RectangleT &child_rectangle = child_tree_node.minimum_bounding_rectangle; - const double current_min_dist = + const float current_min_dist = child_rectangle.GetMinDist(input_coordinate); - const double current_min_max_dist = + const float current_min_max_dist = child_rectangle.GetMinMaxDist(input_coordinate); if (current_min_max_dist < min_max_dist) { @@ -597,19 +597,19 @@ class StaticRTree const bool ignore_tiny_components = (zoom_level <= 14); DataT nearest_edge; - double min_dist = std::numeric_limits::max(); - double min_max_dist = std::numeric_limits::max(); + float min_dist = std::numeric_limits::max(); + float min_max_dist = std::numeric_limits::max(); bool found_a_nearest_edge = false; FixedPointCoordinate nearest, current_start_coordinate, current_end_coordinate; // initialize queue with root element std::priority_queue traversal_queue; - double current_min_dist = + float current_min_dist = m_search_tree[0].minimum_bounding_rectangle.GetMinDist(input_coordinate); traversal_queue.emplace(0, current_min_dist); - BOOST_ASSERT_MSG(std::numeric_limits::epsilon() > + BOOST_ASSERT_MSG(std::numeric_limits::epsilon() > (0. - traversal_queue.top().min_dist), "Root element in NN Search has min dist != 0."); @@ -635,8 +635,8 @@ class StaticRTree continue; } - double current_ratio = 0.; - const double current_perpendicular_distance = + float current_ratio = 0.; + const float current_perpendicular_distance = FixedPointCoordinate::ComputePerpendicularDistance( m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v), @@ -647,7 +647,7 @@ class StaticRTree BOOST_ASSERT(0. <= current_perpendicular_distance); if ((current_perpendicular_distance < min_dist) && - !DoubleEpsilonCompare(current_perpendicular_distance, min_dist)) + !EpsilonCompare(current_perpendicular_distance, min_dist)) { // found a new minimum min_dist = current_perpendicular_distance; // TODO: use assignment c'tor in PhantomNode @@ -685,9 +685,9 @@ class StaticRTree const int32_t child_id = current_tree_node.children[i]; TreeNode &child_tree_node = m_search_tree[child_id]; RectangleT &child_rectangle = child_tree_node.minimum_bounding_rectangle; - const double current_min_dist = + const float current_min_dist = child_rectangle.GetMinDist(input_coordinate); - const double current_min_max_dist = + const float current_min_max_dist = child_rectangle.GetMinMaxDist(input_coordinate); if (current_min_max_dist < min_max_dist) { @@ -717,18 +717,18 @@ class StaticRTree result_phantom_node.location.lat = input_coordinate.lat; } - double ratio = 0.; + float ratio = 0.f; if (found_a_nearest_edge) { - const double distance_1 = FixedPointCoordinate::ApproximateEuclideanDistance( + const float distance_1 = FixedPointCoordinate::ApproximateEuclideanDistance( current_start_coordinate, result_phantom_node.location); - const double distance_2 = FixedPointCoordinate::ApproximateEuclideanDistance( + const float distance_2 = FixedPointCoordinate::ApproximateEuclideanDistance( current_start_coordinate, current_end_coordinate); ratio = distance_1 / distance_2; - ratio = std::min(1., ratio); + ratio = std::min(1.f, ratio); if (SPECIAL_NODEID != result_phantom_node.forward_node_id) { @@ -768,9 +768,10 @@ class StaticRTree return (a == b && c == d) || (a == c && b == d) || (a == d && b == c); } - inline bool DoubleEpsilonCompare(const double d1, const double d2) const + template + inline bool EpsilonCompare(const FloatT d1, const FloatT d2) const { - return (std::abs(d1 - d2) < std::numeric_limits::epsilon()); + return (std::abs(d1 - d2) < std::numeric_limits::epsilon()); } }; diff --git a/Include/osrm/Coordinate.h b/Include/osrm/Coordinate.h index b1823f9e2..343133f12 100644 --- a/Include/osrm/Coordinate.h +++ b/Include/osrm/Coordinate.h @@ -31,8 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include //for std::ostream -constexpr double COORDINATE_PRECISION = 1000000.; -typedef std::pair Point; +constexpr float COORDINATE_PRECISION = 1000000.; struct FixedPointCoordinate { @@ -53,10 +52,12 @@ struct FixedPointCoordinate const FixedPointCoordinate &c2); static float ApproximateEuclideanDistance(const FixedPointCoordinate &c1, - const FixedPointCoordinate &c2); + const FixedPointCoordinate &c2); - static float - ApproximateEuclideanDistance(const int lat1, const int lon1, const int lat2, const int lon2); + static float ApproximateEuclideanDistance(const int lat1, const int lon1, const int lat2, const int lon2); + + static float ApproximateSquaredEuclideanDistance(const FixedPointCoordinate &c1, + const FixedPointCoordinate &c2); static void convertInternalLatLonToString(const int value, std::string &output); @@ -66,24 +67,24 @@ struct FixedPointCoordinate static void convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord, std::string &output); - static double ComputePerpendicularDistance(const FixedPointCoordinate &point, + static float ComputePerpendicularDistance(const FixedPointCoordinate &point, const FixedPointCoordinate &segA, const FixedPointCoordinate &segB); - static double ComputePerpendicularDistance(const FixedPointCoordinate &coord_a, + static float ComputePerpendicularDistance(const FixedPointCoordinate &coord_a, const FixedPointCoordinate &coord_b, const FixedPointCoordinate &query_location, FixedPointCoordinate &nearest_location, - double &r); + float &r); - static double GetBearing(const FixedPointCoordinate &A, const FixedPointCoordinate &B); + static float GetBearing(const FixedPointCoordinate &A, const FixedPointCoordinate &B); - double GetBearing(const FixedPointCoordinate &other) const; + float GetBearing(const FixedPointCoordinate &other) const; void Output(std::ostream &out) const; - static double DegreeToRadian(const double degree); - static double RadianToDegree(const double radian); + static float DegreeToRadian(const float degree); + static float RadianToDegree(const float radian); }; inline std::ostream &operator<<(std::ostream &o, FixedPointCoordinate const &c) diff --git a/Util/MercatorUtil.h b/Util/MercatorUtil.h index 5a3505087..9cae9309e 100644 --- a/Util/MercatorUtil.h +++ b/Util/MercatorUtil.h @@ -28,16 +28,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MERCATOR_UTIL_H #define MERCATOR_UTIL_H +#include "SimpleLogger.h" + #include -inline double y2lat(const double a) +inline float y2lat(const float a) { - return 180. / M_PI * (2. * atan(exp(a * M_PI / 180.)) - M_PI / 2.); + return 180.f / M_PI * (2.f * std::atan(std::exp(a * M_PI / 180.f)) - M_PI / 2.f); } -inline double lat2y(const double a) +inline float lat2y(const float a) { - return 180. / M_PI * log(tan(M_PI / 4. + a * (M_PI / 180.) / 2.)); + return 180.f / M_PI * log(tan(M_PI / 4.f + a * (M_PI / 180.f) / 2.f)); } #endif // MERCATOR_UTIL_H diff --git a/features/testbot/projection.feature b/features/testbot/projection.feature index 5b35f47bf..b66f03d52 100644 --- a/features/testbot/projection.feature +++ b/features/testbot/projection.feature @@ -29,7 +29,7 @@ Feature: Projection to nearest point on road | a | d | abc | NE | 45 | 1000m +-7 | | d | a | abc | SW | 225 | 1000m +-7 | | c | d | abc | SW | 225 | 1000m +-7 | - | d | c | abc | NE | 45 +-1 | 1000m +-7 | + | d | c | abc | NE | 45 +-2 | 1000m +-7 | Scenario: Projection onto way at high latitudes, no distance When I route I should get