From ce60af502953049eaacf5b15f1faeac30006397e Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Wed, 8 Jan 2014 17:18:59 +0100 Subject: [PATCH] fixes issues #770 and #856 --- DataStructures/EdgeBasedNode.h | 59 ++++++++++++++++++++-------------- DataStructures/StaticRTree.h | 37 ++++++++++++++------- 2 files changed, 59 insertions(+), 37 deletions(-) diff --git a/DataStructures/EdgeBasedNode.h b/DataStructures/EdgeBasedNode.h index 830b5c675..5c5e6ed5f 100644 --- a/DataStructures/EdgeBasedNode.h +++ b/DataStructures/EdgeBasedNode.h @@ -3,7 +3,10 @@ #include +#include + #include "../Util/MercatorUtil.h" +#include "../Util/SimpleLogger.h" #include "../typedefs.h" #include @@ -24,22 +27,24 @@ struct EdgeBasedNode { double ComputePerpendicularDistance( - const FixedPointCoordinate& inputPoint, + const FixedPointCoordinate& query_location, FixedPointCoordinate & nearest_location, double & r ) const { + BOOST_ASSERT( query_location.isValid() ); + if( ignoreInGrid ) { return std::numeric_limits::max(); } - const double x = lat2y(inputPoint.lat/COORDINATE_PRECISION); - const double y = inputPoint.lon/COORDINATE_PRECISION; + const double x = lat2y(query_location.lat/COORDINATE_PRECISION); + const double y = query_location.lon/COORDINATE_PRECISION; const double a = lat2y(lat1/COORDINATE_PRECISION); const double b = lon1/COORDINATE_PRECISION; const double c = lat2y(lat2/COORDINATE_PRECISION); const double d = lon2/COORDINATE_PRECISION; - double p,q,mX,nY; - if(std::fabs(a-c) > std::numeric_limits::epsilon() ){ + 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); @@ -49,30 +54,38 @@ struct EdgeBasedNode { q = y; } nY = (d*p - c*q)/(a*d - b*c); - mX = (p - nY*a)/c;// These values are actually n/m+n and m/m+n , we need + 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(mX)) { - r = ((lat2 == inputPoint.lat) && (lon2 == inputPoint.lon)) ? 1. : 0.; - } else { - r = mX; + if( std::isnan(r) ) { + r = ((lat2 == query_location.lat) && (lon2 == 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.; } - if(r<=0.){ + BOOST_ASSERT( !std::isnan(r) ); + if( r <= 0. ){ nearest_location.lat = lat1; nearest_location.lon = lon1; - return ((b - y)*(b - y) + (a - x)*(a - x)); -// return std::sqrt(((b - y)*(b - y) + (a - x)*(a - x))); - } else if(r >= 1.){ + } else if( r >= 1. ){ nearest_location.lat = lat2; nearest_location.lon = lon2; - return ((d - y)*(d - y) + (c - x)*(c - x)); -// return std::sqrt(((d - y)*(d - y) + (c - x)*(c - x))); + } else { + // point lies in between + nearest_location.lat = y2lat(p)*COORDINATE_PRECISION; + nearest_location.lon = q*COORDINATE_PRECISION; } - // point lies in between - nearest_location.lat = y2lat(p)*COORDINATE_PRECISION; - nearest_location.lon = q*COORDINATE_PRECISION; -// return std::sqrt((p-x)*(p-x) + (q-y)*(q-y)); - return (p-x)*(p-x) + (q-y)*(q-y); + 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; } bool operator<(const EdgeBasedNode & other) const { @@ -92,10 +105,6 @@ struct EdgeBasedNode { return centroid; } - // inline bool isIgnored() const { - // return ignoreInGrid; - // } - NodeID id; int lat1; int lat2; diff --git a/DataStructures/StaticRTree.h b/DataStructures/StaticRTree.h index 02647c277..9c727fb46 100644 --- a/DataStructures/StaticRTree.h +++ b/DataStructures/StaticRTree.h @@ -786,7 +786,8 @@ public: nearest, current_ratio ); - BOOST_ASSERT( 0 <= current_perpendicular_distance ); + + BOOST_ASSERT( 0. <= current_perpendicular_distance ); if( ( current_perpendicular_distance < min_dist ) && @@ -856,17 +857,6 @@ public: } } - const double ratio = (found_a_nearest_edge ? - std::min(1., FixedPointCoordinate::ApproximateDistance(current_start_coordinate, - result_phantom_node.location)/FixedPointCoordinate::ApproximateDistance(current_start_coordinate, current_end_coordinate) - ) : 0 - ); - result_phantom_node.weight1 *= ratio; - if(INT_MAX != result_phantom_node.weight2) { - result_phantom_node.weight2 *= (1.-ratio); - } - result_phantom_node.ratio = ratio; - //Hack to fix rounding errors and wandering via nodes. if(std::abs(input_coordinate.lon - result_phantom_node.location.lon) == 1) { result_phantom_node.location.lon = input_coordinate.lon; @@ -875,6 +865,29 @@ public: result_phantom_node.location.lat = input_coordinate.lat; } + double ratio = 0.; + + if( found_a_nearest_edge) { + const double distance_1 = FixedPointCoordinate::ApproximateDistance( + current_start_coordinate, + result_phantom_node.location + ); + + const double distance_2 = FixedPointCoordinate::ApproximateDistance( + current_start_coordinate, + current_end_coordinate + ); + + ratio = distance_1/distance_2; + ratio = std::min(1., ratio); + } + + result_phantom_node.weight1 *= ratio; + if(INT_MAX != result_phantom_node.weight2) { + result_phantom_node.weight2 *= (1.-ratio); + } + result_phantom_node.ratio = ratio; + return found_a_nearest_edge; }