add approximator for perpendicular distance

DataStructures/Coordinate.cpp

@@ -385,6 +385,141 @@ double FixedPointCoordinate::RadianToDegree(const double radian)
     return radian * (180. / M_PI);
 }
 
+
+double
+FixedPointCoordinate::ApproximatePerpendicularDistance(const FixedPointCoordinate &coord_a,
+                                                       const FixedPointCoordinate &coord_b,
+                                                       const FixedPointCoordinate &query_location,
+                                                       FixedPointCoordinate &nearest_location,
+                                                       double &ratio,
+                                                       double precision) const
+{
+    BOOST_ASSERT(query_location.isValid());
+
+    const double epsilon = 1.0 / precision;
+
+    // p, q : the end points of the underlying edge
+    const Point p(lat2y(coord_a.lat / COORDINATE_PRECISION), coord_a.lon / COORDINATE_PRECISION);
+    const Point q(lat2y(coord_b.lat / COORDINATE_PRECISION), coord_b.lon / COORDINATE_PRECISION);
+
+    // r : query location
+    const Point r(lat2y(query_location.lat / COORDINATE_PRECISION),
+                  query_location.lon / COORDINATE_PRECISION);
+
+    const Point foot = ComputePerpendicularFoot(p, q, r, epsilon);
+    ratio = ComputeRatio(p, q, foot, epsilon);
+
+    BOOST_ASSERT(!std::isnan(ratio));
+
+    nearest_location = ComputeNearestPointOnSegment(coord_a, coord_b, foot, ratio);
+
+    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::ApproximateEuclideanDistance(query_location, nearest_location);
+
+    BOOST_ASSERT(0.0 <= approximated_distance);
+    return approximated_distance;
+}
+
+// Compute the perpendicular foot of point r on the line defined by p and q.
+Point FixedPointCoordinate::ComputePerpendicularFoot(const Point &p,
+                                                     const Point &q,
+                                                     const Point &r,
+                                                     double epsilon) const
+{
+
+    // the projection of r onto the line pq
+    double foot_x, foot_y;
+
+    const bool is_parallel_to_y_axis = std::abs(q.first - p.first) < epsilon;
+
+    if (is_parallel_to_y_axis)
+    {
+        foot_x = q.first;
+        foot_y = r.second;
+    }
+    else
+    {
+        // the slope of the line through (a|b) and (c|d)
+        const double m = (q.second - p.second) / (q.first - p.first);
+
+        // Projection of (x|y) onto the line joining (a|b) and (c|d).
+        foot_x = ((r.first + (m * r.second)) + (m * m * p.first - m * p.second)) / (1.0 + m * m);
+        foot_y = p.second + m * (foot_x - p.first);
+    }
+
+    return Point(foot_x, foot_y);
+}
+
+// Compute the ratio of the line segment pr to line segment pq.
+double FixedPointCoordinate::ComputeRatio(const Point &p,
+                                          const Point &q,
+                                          const Point &r,
+                                          double epsilon) const
+{
+
+    const bool is_parallel_to_x_axis = std::abs(q.second - p.second) < epsilon;
+    const bool is_parallel_to_y_axis = std::abs(q.first - p.first) < epsilon;
+
+    double ratio;
+
+    if (!is_parallel_to_y_axis)
+    {
+        ratio = (r.first - p.first) / (q.first - p.first);
+    }
+    else if (!is_parallel_to_x_axis)
+    {
+        ratio = (r.second - p.second) / (q.second - p.second);
+    }
+    else
+    {
+        // (a|b) and (c|d) are essentially the same point
+        // by convention, we set the ratio to 0 in this case
+        // ratio = ((lat2 == query_location.lat) && (lon2 == query_location.lon)) ? 1. : 0.;
+        ratio = 0.0;
+    }
+
+    // Round to integer if the ratio is close to 0 or 1.
+    if (std::abs(ratio) <= epsilon)
+    {
+        ratio = 0.0;
+    }
+    else if (std::abs(ratio - 1.0) <= epsilon)
+    {
+        ratio = 1.0;
+    }
+
+    return ratio;
+}
+
+// Computes the point on the segment pq which is nearest to a point r = p + lambda * (q-p).
+// p and q are the end points of the underlying edge.
+FixedPointCoordinate FixedPointCoordinate::ComputeNearestPointOnSegment(const FixedPointCoordinate &coord_a,
+                                                                        const FixedPointCoordinate &coord_b,
+                                                                        const Point &r,
+                                                                        double lambda) const
+{
+
+    if (lambda <= 0.0)
+    {
+        return coord_a;
+    }
+    else if (lambda >= 1.0)
+    {
+        return coord_b;
+    }
+    else
+    {
+        // r lies between p and q
+        return FixedPointCoordinate(y2lat(r.first) * COORDINATE_PRECISION,
+                                    r.second * COORDINATE_PRECISION);
+    }
+}
+
+
 // double PointSegmentDistanceSquared( double px, double py,
 //                                     double p1x, double p1y,
 //                                     double p2x, double p2y,

Include/osrm/Coordinate.h

@@ -28,9 +28,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef FIXED_POINT_COORDINATE_H_
 #define FIXED_POINT_COORDINATE_H_
 
+#include <functional>
 #include <iosfwd> //for std::ostream
 
 constexpr double COORDINATE_PRECISION = 1000000.;
+typedef std::pair<double,double> Point;
 
 struct FixedPointCoordinate
 {
@@ -82,6 +84,21 @@ struct FixedPointCoordinate
 
     static double DegreeToRadian(const double degree);
     static double RadianToDegree(const double radian);
+
+
+    Point ComputePerpendicularFoot(const Point &p, const Point &q, const Point &r, double epsilon) const;
+    double ComputeRatio(const Point & p, const Point & q, const Point & r, double epsilon) const ;
+    FixedPointCoordinate ComputeNearestPointOnSegment(const FixedPointCoordinate &coord_a,
+                                                      const FixedPointCoordinate &coord_b,
+                                                      const Point & r, double lambda) const;
+    double ApproximatePerpendicularDistance(const FixedPointCoordinate &coord_a,
+                                            const FixedPointCoordinate &coord_b,
+                                            const FixedPointCoordinate &query_location,
+                                            FixedPointCoordinate & nearest_location,
+                                            double & ratio,
+                                            double precision = COORDINATE_PRECISION
+                                            ) const;
+
 };
 
 inline std::ostream &operator<<(std::ostream &o, FixedPointCoordinate const &c)