fixes issues #770 and #856

DataStructures/EdgeBasedNode.h

@@ -3,7 +3,10 @@
 
 #include <cmath>
 
+#include <boost/assert.hpp>
+
 #include "../Util/MercatorUtil.h"
+#include "../Util/SimpleLogger.h"
 #include "../typedefs.h"
 
 #include <osrm/Coordinate.h>
@@ -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<double>::max();
         }
 
-        const double x = lat2y(inputPoint.lat/COORDINATE_PRECISION);
+        const double x = lat2y(query_location.lat/COORDINATE_PRECISION);
-        const double y = inputPoint.lon/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;
+        double p,q/*,mX*/,nY;
-        if(std::fabs(a-c) > std::numeric_limits<double>::epsilon() ){
+        if( std::abs(a-c) > std::numeric_limits<double>::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)) {
+        if( std::isnan(r) ) {
-            r = ((lat2 == inputPoint.lat) && (lon2 == inputPoint.lon)) ? 1. : 0.;
+            r = ((lat2 == query_location.lat) && (lon2 == query_location.lon)) ? 1. : 0.;
-        } else {
+        } else if( std::abs(r) <= std::numeric_limits<double>::epsilon() ) {
-            r = mX;
+            r = 0.;
+        } else if( std::abs(r-1.) <= std::numeric_limits<double>::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));
+        } else if( r >= 1. ){
-//            return std::sqrt(((b - y)*(b - y) + (a - x)*(a - x)));
-        } else if(r >= 1.){
             nearest_location.lat = lat2;
             nearest_location.lon = lon2;
-            return ((d - y)*(d - y) + (c - x)*(c - x));
+        } else {
-//            return std::sqrt(((d - y)*(d - y) + (c - x)*(c - x)));
-        }
             // 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;

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;
     }