From e3cc896a427baf6518764ffb9305c06994f21b49 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Wed, 30 Apr 2014 14:32:56 +0200 Subject: [PATCH] consolidate duplicated distance calculations --- Algorithms/DouglasPeucker.cpp | 203 +++++++-------------- Algorithms/DouglasPeucker.h | 37 ++-- DataStructures/Coordinate.cpp | 316 ++++++++++++++++++++++++--------- DataStructures/EdgeBasedNode.h | 72 +------- DataStructures/StaticRTree.h | 2 +- Include/osrm/Coordinate.h | 61 +++---- 6 files changed, 335 insertions(+), 356 deletions(-) diff --git a/Algorithms/DouglasPeucker.cpp b/Algorithms/DouglasPeucker.cpp index a45484e2b..133f04629 100644 --- a/Algorithms/DouglasPeucker.cpp +++ b/Algorithms/DouglasPeucker.cpp @@ -25,173 +25,96 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include + #include "DouglasPeucker.h" #include "../DataStructures/SegmentInformation.h" -#include "../Util/MercatorUtil.h" + +#include #include -//These thresholds are more or less heuristically chosen. -static double DouglasPeuckerThresholds[19] = { - 262144., //z0 - 131072., //z1 - 65536., //z2 - 32768., //z3 - 16384., //z4 - 8192., //z5 - 4096., //z6 - 2048., //z7 - 960., //z8 - 480., //z9 - 240., //z10 - 90., //z11 - 50., //z12 - 25., //z13 - 15., //z14 - 5., //z15 - .65, //z16 - .5, //z17 - .35 //z18 +// These thresholds are more or less heuristically chosen. +static double DouglasPeuckerThresholds[19] = {262144., // z0 + 131072., // z1 + 65536., // z2 + 32768., // z3 + 16384., // z4 + 8192., // z5 + 4096., // z6 + 2048., // z7 + 960., // z8 + 480., // z9 + 240., // z10 + 90., // z11 + 50., // z12 + 25., // z13 + 15., // z14 + 5., // z15 + .65, // z16 + .5, // z17 + .35 // z18 }; -/** - * Yuck! Code duplication. This function is also in EgdeBasedNode.h - */ -double DouglasPeucker::ComputeDistance( - const FixedPointCoordinate& point, - const FixedPointCoordinate& segA, - const FixedPointCoordinate& segB -) const { - 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 double 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); - } else { - p = c; - q = y; - } - nY = (d*p - c*q)/(a*d - b*c); - - //discretize the result to coordinate precision. it's a hack! - if( std::abs(nY) < (1./COORDINATE_PRECISION) ) { - nY = 0.; - } - - double 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() ) { - r = 0.; - } else if( std::abs(r-1.) <= std::numeric_limits::epsilon() ) { - r = 1.; - } - FixedPointCoordinate nearest_location; - BOOST_ASSERT( !std::isnan(r) ); - if( r <= 0. ){ - nearest_location.lat = segA.lat; - nearest_location.lon = segA.lon; - } else if( r >= 1. ){ - nearest_location.lat = segB.lat; - nearest_location.lon = segB.lon; - } else { // point lies in between - nearest_location.lat = y2lat(p)*COORDINATE_PRECISION; - nearest_location.lon = q*COORDINATE_PRECISION; - } - BOOST_ASSERT( nearest_location.isValid() ); - const double approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance( - point, - nearest_location - ); - BOOST_ASSERT( 0. <= approximated_distance ); - return approximated_distance; -} - -void DouglasPeucker::Run( - std::vector & input_geometry, - const unsigned zoom_level -) { +void DouglasPeucker::Run(std::vector &input_geometry, const unsigned zoom_level) +{ { BOOST_ASSERT_MSG(zoom_level < 19, "unsupported zoom level"); BOOST_ASSERT_MSG(1 < input_geometry.size(), "geometry invalid"); - std::size_t left_border = 0; - std::size_t right_border = 1; - //Sweep over array and identify those ranges that need to be checked - do { - BOOST_ASSERT_MSG( - input_geometry[left_border].necessary, - "left border must be necessary" - ); - BOOST_ASSERT_MSG( - input_geometry.back().necessary, - "right border must be necessary" - ); + unsigned left_border = 0; + unsigned right_border = 1; + // Sweep over array and identify those ranges that need to be checked + do + { + BOOST_ASSERT_MSG(input_geometry[left_border].necessary, + "left border must be necessary"); + BOOST_ASSERT_MSG(input_geometry.back().necessary, "right border must be necessary"); - if(input_geometry[right_border].necessary) { + if (input_geometry[right_border].necessary) + { recursion_stack.push(std::make_pair(left_border, right_border)); left_border = right_border; } ++right_border; - } while( right_border < input_geometry.size()); + } while (right_border < input_geometry.size()); } - while( !recursion_stack.empty() ) { - //pop next element - const PairOfPoints pair = recursion_stack.top(); + while (!recursion_stack.empty()) + { + // pop next element + const GeometryRange pair = recursion_stack.top(); recursion_stack.pop(); - BOOST_ASSERT_MSG( - input_geometry[pair.first].necessary, - "left border mus be necessary" - ); - BOOST_ASSERT_MSG( - input_geometry[pair.second].necessary, - "right border must be necessary" - ); - BOOST_ASSERT_MSG( - pair.second < input_geometry.size(), - "right border outside of geometry" - ); - BOOST_ASSERT_MSG( - pair.first < pair.second, - "left border on the wrong side" - ); + BOOST_ASSERT_MSG(input_geometry[pair.first].necessary, "left border mus be necessary"); + BOOST_ASSERT_MSG(input_geometry[pair.second].necessary, "right border must be necessary"); + BOOST_ASSERT_MSG(pair.second < input_geometry.size(), "right border outside of geometry"); + BOOST_ASSERT_MSG(pair.first < pair.second, "left border on the wrong side"); double max_distance = std::numeric_limits::min(); - std::size_t farthest_element_index = pair.second; - //find index idx of element with max_distance - for(std::size_t i = pair.first+1; i < pair.second; ++i){ - const int temp_dist = ComputeDistance( - input_geometry[i].location, - input_geometry[pair.first].location, - input_geometry[pair.second].location - ); + unsigned farthest_element_index = pair.second; + // find index idx of element with max_distance + for (unsigned i = pair.first + 1; i < pair.second; ++i) + { + const int temp_dist = FixedPointCoordinate::ComputePerpendicularDistance( + input_geometry[i].location, + input_geometry[pair.first].location, + input_geometry[pair.second].location); const double distance = std::abs(temp_dist); - if( - distance > DouglasPeuckerThresholds[zoom_level] && - distance > max_distance - ) { + if (distance > DouglasPeuckerThresholds[zoom_level] && distance > max_distance) + { farthest_element_index = i; max_distance = distance; } } - if (max_distance > DouglasPeuckerThresholds[zoom_level]) { + if (max_distance > DouglasPeuckerThresholds[zoom_level]) + { // mark idx as necessary input_geometry[farthest_element_index].necessary = true; - if (1 < (farthest_element_index - pair.first) ) { - recursion_stack.push( - std::make_pair(pair.first, farthest_element_index) - ); + if (1 < (farthest_element_index - pair.first)) + { + recursion_stack.push(std::make_pair(pair.first, farthest_element_index)); } - if (1 < (pair.second - farthest_element_index) ) { - recursion_stack.push( - std::make_pair(farthest_element_index, pair.second) - ); + if (1 < (pair.second - farthest_element_index)) + { + recursion_stack.push(std::make_pair(farthest_element_index, pair.second)); } } } diff --git a/Algorithms/DouglasPeucker.h b/Algorithms/DouglasPeucker.h index 6f7ecb0e4..4e5966467 100644 --- a/Algorithms/DouglasPeucker.h +++ b/Algorithms/DouglasPeucker.h @@ -28,16 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef DOUGLASPEUCKER_H_ #define DOUGLASPEUCKER_H_ -#include "../Util/SimpleLogger.h" - -#include - -#include -#include - -#include - -#include #include #include @@ -48,25 +38,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * bit indicating if the points is present in the generalization. * Note: points may also be pre-selected*/ +struct FixedPointCoordinate; struct SegmentInformation; -class DouglasPeucker { -private: - typedef std::pair PairOfPoints; - //Stack to simulate the recursion - std::stack recursion_stack; +class DouglasPeucker +{ + private: + typedef std::pair GeometryRange; + // Stack to simulate the recursion + std::stack recursion_stack; - double ComputeDistance( - const FixedPointCoordinate& point, - const FixedPointCoordinate& segA, - const FixedPointCoordinate& segB - ) const; -public: - void Run( - std::vector & input_geometry, - const unsigned zoom_level - ); + double ComputeDistance(const FixedPointCoordinate &point, + const FixedPointCoordinate &segA, + const FixedPointCoordinate &segB) const; + public: + void Run(std::vector &input_geometry, const unsigned zoom_level); }; #endif /* DOUGLASPEUCKER_H_ */ diff --git a/DataStructures/Coordinate.cpp b/DataStructures/Coordinate.cpp index 1384bd8f8..b9d0628ed 100644 --- a/DataStructures/Coordinate.cpp +++ b/DataStructures/Coordinate.cpp @@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include +#include "../Util/MercatorUtil.h" #include "../Util/SimpleLogger.h" #include "../Util/StringUtil.h" @@ -38,21 +39,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include FixedPointCoordinate::FixedPointCoordinate() - : lat(std::numeric_limits::min()), - lon(std::numeric_limits::min()) -{ } + : lat(std::numeric_limits::min()), lon(std::numeric_limits::min()) +{ +} -FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) - : lat(lat), - lon(lon) +FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) : lat(lat), lon(lon) { #ifndef NDEBUG - if(0 != (std::abs(lat) >> 30)) + if (0 != (std::abs(lat) >> 30)) { std::bitset<32> y(lat); SimpleLogger().Write(logDEBUG) << "broken lat: " << lat << ", bits: " << y; } - if(0 != (std::abs(lon) >> 30)) + if (0 != (std::abs(lon) >> 30)) { std::bitset<32> x(lon); SimpleLogger().Write(logDEBUG) << "broken lon: " << lon << ", bits: " << x; @@ -60,124 +59,265 @@ FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) #endif } -void FixedPointCoordinate::Reset() { +void FixedPointCoordinate::Reset() +{ lat = std::numeric_limits::min(); lon = std::numeric_limits::min(); } -bool FixedPointCoordinate::isSet() const { - return (std::numeric_limits::min() != lat) && - (std::numeric_limits::min() != lon); +bool FixedPointCoordinate::isSet() const +{ + return (std::numeric_limits::min() != lat) && (std::numeric_limits::min() != lon); } -bool FixedPointCoordinate::isValid() const { - if (lat > 90*COORDINATE_PRECISION || - lat < -90*COORDINATE_PRECISION || - lon > 180*COORDINATE_PRECISION || - lon < -180*COORDINATE_PRECISION) +bool FixedPointCoordinate::isValid() const +{ + if (lat > 90 * COORDINATE_PRECISION || lat < -90 * COORDINATE_PRECISION || + lon > 180 * COORDINATE_PRECISION || lon < -180 * COORDINATE_PRECISION) { return false; } return true; } -bool FixedPointCoordinate::operator==(const FixedPointCoordinate & other) const { +bool FixedPointCoordinate::operator==(const FixedPointCoordinate &other) const +{ return lat == other.lat && lon == other.lon; } -double FixedPointCoordinate::ApproximateDistance( - const int lat1, - const int lon1, - const int lat2, - const int lon2 -) { +double FixedPointCoordinate::ApproximateDistance(const int lat1, + const int lon1, + const int lat2, + const int lon2) +{ BOOST_ASSERT(lat1 != std::numeric_limits::min()); BOOST_ASSERT(lon1 != std::numeric_limits::min()); BOOST_ASSERT(lat2 != std::numeric_limits::min()); BOOST_ASSERT(lon2 != std::numeric_limits::min()); double RAD = 0.017453292519943295769236907684886; - double lt1 = lat1/COORDINATE_PRECISION; - double ln1 = lon1/COORDINATE_PRECISION; - double lt2 = lat2/COORDINATE_PRECISION; - double ln2 = lon2/COORDINATE_PRECISION; - double dlat1=lt1*(RAD); + double lt1 = lat1 / COORDINATE_PRECISION; + double ln1 = lon1 / COORDINATE_PRECISION; + double lt2 = lat2 / COORDINATE_PRECISION; + double ln2 = lon2 / COORDINATE_PRECISION; + double dlat1 = lt1 * (RAD); - double dlong1=ln1*(RAD); - double dlat2=lt2*(RAD); - double dlong2=ln2*(RAD); + double dlong1 = ln1 * (RAD); + double dlat2 = lt2 * (RAD); + double dlong2 = ln2 * (RAD); - double dLong=dlong1-dlong2; - double dLat=dlat1-dlat2; + double dLong = dlong1 - dlong2; + double dLat = dlat1 - dlat2; - double aHarv= pow(sin(dLat/2.0),2.0)+cos(dlat1)*cos(dlat2)*pow(sin(dLong/2.),2); - double cHarv=2.*atan2(sqrt(aHarv),sqrt(1.0-aHarv)); - //earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi) - //The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles) - const double earth=6372797.560856; - return earth*cHarv; + double aHarv = pow(sin(dLat / 2.0), 2.0) + cos(dlat1) * cos(dlat2) * pow(sin(dLong / 2.), 2); + double cHarv = 2. * atan2(sqrt(aHarv), sqrt(1.0 - aHarv)); + // earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi) + // The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles) + const double earth = 6372797.560856; + return earth * cHarv; } -double FixedPointCoordinate::ApproximateDistance( - const FixedPointCoordinate &c1, - const FixedPointCoordinate &c2 -) { +double FixedPointCoordinate::ApproximateDistance(const FixedPointCoordinate &c1, + const FixedPointCoordinate &c2) +{ return ApproximateDistance(c1.lat, c1.lon, c2.lat, c2.lon); } -double FixedPointCoordinate::ApproximateEuclideanDistance( - const FixedPointCoordinate &c1, - const FixedPointCoordinate &c2 -) { +double FixedPointCoordinate::ApproximateEuclideanDistance(const FixedPointCoordinate &c1, + const FixedPointCoordinate &c2) +{ BOOST_ASSERT(c1.lat != std::numeric_limits::min()); BOOST_ASSERT(c1.lon != std::numeric_limits::min()); BOOST_ASSERT(c2.lat != std::numeric_limits::min()); BOOST_ASSERT(c2.lon != std::numeric_limits::min()); const double RAD = 0.017453292519943295769236907684886; - const double lat1 = (c1.lat/COORDINATE_PRECISION)*RAD; - const double lon1 = (c1.lon/COORDINATE_PRECISION)*RAD; - const double lat2 = (c2.lat/COORDINATE_PRECISION)*RAD; - const double lon2 = (c2.lon/COORDINATE_PRECISION)*RAD; + const double lat1 = (c1.lat / COORDINATE_PRECISION) * RAD; + const double lon1 = (c1.lon / COORDINATE_PRECISION) * RAD; + const double lat2 = (c2.lat / COORDINATE_PRECISION) * RAD; + const double lon2 = (c2.lon / COORDINATE_PRECISION) * RAD; - const double x = (lon2-lon1) * cos((lat1+lat2)/2.); - const double y = (lat2-lat1); + const double x = (lon2 - lon1) * cos((lat1 + lat2) / 2.); + const double y = (lat2 - lat1); const double earthRadius = 6372797.560856; - return sqrt(x*x + y*y) * earthRadius; + return sqrt(x * x + y * y) * earthRadius; } -void FixedPointCoordinate::convertInternalLatLonToString( - const int value, - std::string & output -) { +// Yuck! Code duplication. This function is also in EgdeBasedNode.h +double 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 double 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); + } + else + { + p = c; + q = y; + } + nY = (d * p - c * q) / (a * d - b * c); + + // discretize the result to coordinate precision. it's a hack! + if (std::abs(nY) < (1. / COORDINATE_PRECISION)) + { + nY = 0.; + } + + double 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()) + { + r = 0.; + } + else if (std::abs(r - 1.) <= std::numeric_limits::epsilon()) + { + r = 1.; + } + FixedPointCoordinate nearest_location; + BOOST_ASSERT(!std::isnan(r)); + if (r <= 0.) + { // point is "left" of edge + nearest_location.lat = segA.lat; + nearest_location.lon = segA.lon; + } + else if (r >= 1.) + { // point is "right" of edge + nearest_location.lat = segB.lat; + nearest_location.lon = segB.lon; + } + else + { // point lies in between + nearest_location.lat = y2lat(p) * COORDINATE_PRECISION; + nearest_location.lon = q * COORDINATE_PRECISION; + } + BOOST_ASSERT(nearest_location.isValid()); + const double approximated_distance = + FixedPointCoordinate::ApproximateDistance(point, nearest_location); + BOOST_ASSERT(0. <= approximated_distance); + return approximated_distance; +} + +double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &coord_a, + const FixedPointCoordinate &coord_b, + const FixedPointCoordinate &query_location, + FixedPointCoordinate &nearest_location, + double &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 double 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); + } + else + { + p = c; + q = y; + } + nY = (d * p - c * q) / (a * d - b * c); + + // discretize the result to coordinate precision. it's a hack! + if (std::abs(nY) < (1. / COORDINATE_PRECISION)) + { + nY = 0.; + } + + r = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need + // not calculate the explicit values of m an n as we + // are just interested in the ratio + if (std::isnan(r)) + { + r = ((coord_b.lat == query_location.lat) && (coord_b.lon == query_location.lon)) ? 1. : 0.; + } + else if (std::abs(r) <= std::numeric_limits::epsilon()) + { + r = 0.; + } + else if (std::abs(r - 1.) <= std::numeric_limits::epsilon()) + { + r = 1.; + } + BOOST_ASSERT(!std::isnan(r)); + if (r <= 0.) + { + nearest_location.lat = coord_a.lat; + nearest_location.lon = coord_a.lon; + } + else if (r >= 1.) + { + nearest_location.lat = coord_b.lat; + nearest_location.lon = coord_b.lon; + } + else + { + // point lies in between + nearest_location.lat = y2lat(p) * COORDINATE_PRECISION; + nearest_location.lon = q * COORDINATE_PRECISION; + } + 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 = + FixedPointCoordinate::ApproximateDistance(query_location, nearest_location); + BOOST_ASSERT(0. <= approximated_distance); + return approximated_distance; +} + +void FixedPointCoordinate::convertInternalLatLonToString(const int value, std::string &output) +{ char buffer[100]; buffer[11] = 0; // zero termination - char* string = printInt< 11, 6 >( buffer, value ); + char *string = printInt<11, 6>(buffer, value); output = string; } -void FixedPointCoordinate::convertInternalCoordinateToString( - const FixedPointCoordinate & coord, - std::string & output -) { - std::string tmp; - tmp.reserve(23); - convertInternalLatLonToString(coord.lon, tmp); - output = tmp; - output += ","; - convertInternalLatLonToString(coord.lat, tmp); - output += tmp; -} - -void FixedPointCoordinate::convertInternalReversedCoordinateToString( - const FixedPointCoordinate & coord, - std::string & output -) { - std::string tmp; - tmp.reserve(23); - convertInternalLatLonToString(coord.lat, tmp); - output = tmp; - output += ","; - convertInternalLatLonToString(coord.lon, tmp); - output += tmp; -} - -void FixedPointCoordinate::Output(std::ostream & out) const +void FixedPointCoordinate::convertInternalCoordinateToString(const FixedPointCoordinate &coord, + std::string &output) { - out << "(" << lat/COORDINATE_PRECISION << "," << lon/COORDINATE_PRECISION << ")"; + std::string tmp; + tmp.reserve(23); + convertInternalLatLonToString(coord.lon, tmp); + output = tmp; + output += ","; + convertInternalLatLonToString(coord.lat, tmp); + output += tmp; +} + +void +FixedPointCoordinate::convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord, + std::string &output) +{ + std::string tmp; + tmp.reserve(23); + convertInternalLatLonToString(coord.lat, tmp); + output = tmp; + output += ","; + convertInternalLatLonToString(coord.lon, tmp); + output += tmp; +} + +void FixedPointCoordinate::Output(std::ostream &out) const +{ + out << "(" << lat / COORDINATE_PRECISION << "," << lon / COORDINATE_PRECISION << ")"; } diff --git a/DataStructures/EdgeBasedNode.h b/DataStructures/EdgeBasedNode.h index d76a4958b..3a3a5a8a6 100644 --- a/DataStructures/EdgeBasedNode.h +++ b/DataStructures/EdgeBasedNode.h @@ -1,7 +1,6 @@ #ifndef EDGE_BASED_NODE_H #define EDGE_BASED_NODE_H -#include "../Util/MercatorUtil.h" #include "../Util/SimpleLogger.h" #include "../typedefs.h" @@ -61,81 +60,14 @@ struct EdgeBasedNode { ); } - inline static double ComputePerpendicularDistance( - const FixedPointCoordinate & coord_a, - const FixedPointCoordinate & coord_b, - const FixedPointCoordinate & query_location, - FixedPointCoordinate & nearest_location, - double & 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 double 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); - } else { - p = c; - q = y; - } - nY = (d*p - c*q)/(a*d - b*c); - - //discretize the result to coordinate precision. it's a hack! - if( std::abs(nY) < (1./COORDINATE_PRECISION) ) { - nY = 0.; - } - - r = (p - nY*a)/c;// These values are actually n/m+n and m/m+n , we need - // not calculate the explicit values of m an n as we - // are just interested in the ratio - if( std::isnan(r) ) { - r = ((coord_b.lat == query_location.lat) && (coord_b.lon == query_location.lon)) ? 1. : 0.; - } else if( std::abs(r) <= std::numeric_limits::epsilon() ) { - r = 0.; - } else if( std::abs(r-1.) <= std::numeric_limits::epsilon() ) { - r = 1.; - } - BOOST_ASSERT( !std::isnan(r) ); - if( r <= 0. ){ - nearest_location.lat = coord_a.lat; - nearest_location.lon = coord_a.lon; - } else if( r >= 1. ){ - nearest_location.lat = coord_b.lat; - nearest_location.lon = coord_b.lon; - } else { - // point lies in between - nearest_location.lat = y2lat(p)*COORDINATE_PRECISION; - nearest_location.lon = q*COORDINATE_PRECISION; - } - 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 = FixedPointCoordinate::ApproximateDistance( - query_location, - nearest_location - ); - BOOST_ASSERT( 0. <= approximated_distance ); - return approximated_distance; - } - static inline FixedPointCoordinate Centroid( const FixedPointCoordinate & a, const FixedPointCoordinate & b ) { FixedPointCoordinate centroid; //The coordinates of the midpoint are given by: - //x = (x1 + x2) /2 and y = (y1 + y2) /2. - centroid.lon = (std::min(a.lon, b.lon) + std::max(a.lon, b.lon))/2; - centroid.lat = (std::min(a.lat, b.lat) + std::max(a.lat, b.lat))/2; + centroid.lat = (a.lat + b.lat)/2; + centroid.lon = (a.lon + b.lon)/2; return centroid; } diff --git a/DataStructures/StaticRTree.h b/DataStructures/StaticRTree.h index b23cba689..1df319bb4 100644 --- a/DataStructures/StaticRTree.h +++ b/DataStructures/StaticRTree.h @@ -672,7 +672,7 @@ public: } double current_ratio = 0.; - const double current_perpendicular_distance = current_edge.ComputePerpendicularDistance( + const double current_perpendicular_distance = FixedPointCoordinate::ComputePerpendicularDistance( m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v), input_coordinate, diff --git a/Include/osrm/Coordinate.h b/Include/osrm/Coordinate.h index 76d158a34..3a33939e0 100644 --- a/Include/osrm/Coordinate.h +++ b/Include/osrm/Coordinate.h @@ -28,57 +28,54 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef FIXED_POINT_COORDINATE_H_ #define FIXED_POINT_COORDINATE_H_ -#include //for std::ostream +#include //for std::ostream static const double COORDINATE_PRECISION = 1000000.; -struct FixedPointCoordinate { +struct FixedPointCoordinate +{ int lat; int lon; FixedPointCoordinate(); - explicit FixedPointCoordinate( int lat, int lon); + explicit FixedPointCoordinate(int lat, int lon); void Reset(); bool isSet() const; bool isValid() const; - bool operator==(const FixedPointCoordinate & other) const; + bool operator==(const FixedPointCoordinate &other) const; - static double ApproximateDistance( - const int lat1, - const int lon1, - const int lat2, - const int lon2 - ); + static double + ApproximateDistance(const int lat1, const int lon1, const int lat2, const int lon2); - static double ApproximateDistance( - const FixedPointCoordinate & c1, - const FixedPointCoordinate & c2 - ); + static double ApproximateDistance(const FixedPointCoordinate &c1, + const FixedPointCoordinate &c2); - static double ApproximateEuclideanDistance( - const FixedPointCoordinate & c1, - const FixedPointCoordinate & c2 - ); + static double ApproximateEuclideanDistance(const FixedPointCoordinate &c1, + const FixedPointCoordinate &c2); - static void convertInternalLatLonToString( - const int value, - std::string & output - ); + static void convertInternalLatLonToString(const int value, std::string &output); - static void convertInternalCoordinateToString( - const FixedPointCoordinate & coord, - std::string & output - ); + static void convertInternalCoordinateToString(const FixedPointCoordinate &coord, + std::string &output); - static void convertInternalReversedCoordinateToString( - const FixedPointCoordinate & coord, - std::string & output - ); + static void convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord, + std::string &output); - void Output(std::ostream & out) const; + static double ComputePerpendicularDistance(const FixedPointCoordinate &point, + const FixedPointCoordinate &segA, + const FixedPointCoordinate &segB); + + static double ComputePerpendicularDistance(const FixedPointCoordinate &coord_a, + const FixedPointCoordinate &coord_b, + const FixedPointCoordinate &query_location, + FixedPointCoordinate &nearest_location, + double &r); + + void Output(std::ostream &out) const; }; -inline std::ostream& operator<<(std::ostream& o, FixedPointCoordinate const & c){ +inline std::ostream &operator<<(std::ostream &o, FixedPointCoordinate const &c) +{ c.Output(o); return o; }