From b453a42f77c4108d462501acfdf6bd244b092a9c Mon Sep 17 00:00:00 2001
From: Patrick Niklaus <patrick.niklaus@student.kit.edu>
Date: Fri, 4 Jul 2014 01:04:24 +0200
Subject: [PATCH] Fixed perpendicular distance calculation of segment endpoint
 is on equator

---
 DataStructures/Coordinate.cpp | 47 +++++++++++++++++++++++++++--------
 1 file changed, 37 insertions(+), 10 deletions(-)

diff --git a/DataStructures/Coordinate.cpp b/DataStructures/Coordinate.cpp
index 9c453e70d..37da2f8f3 100644
--- a/DataStructures/Coordinate.cpp
+++ b/DataStructures/Coordinate.cpp
@@ -159,10 +159,10 @@ FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &s
     // initialize values
     const float x_value = lat2y(point.lat / COORDINATE_PRECISION);
     const float y_value = point.lon / COORDINATE_PRECISION;
-    const float a = lat2y(source_coordinate.lat / COORDINATE_PRECISION);
-    const float b = source_coordinate.lon / COORDINATE_PRECISION;
-    const float c = lat2y(target_coordinate.lat / COORDINATE_PRECISION);
-    const float d = target_coordinate.lon / COORDINATE_PRECISION;
+    float a = lat2y(source_coordinate.lat / COORDINATE_PRECISION);
+    float b = source_coordinate.lon / COORDINATE_PRECISION;
+    float c = lat2y(target_coordinate.lat / COORDINATE_PRECISION);
+    float d = target_coordinate.lon / COORDINATE_PRECISION;
     float p, q;
     if (std::abs(a - c) > std::numeric_limits<float>::epsilon())
     {
@@ -178,15 +178,35 @@ FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &s
         q = y_value;
     }
 
-    float nY = (d * p - c * q) / (a * d - b * c);
-    // discretize the result to coordinate precision. it's a hack!
-    if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
+    float ratio;
+    bool inverse_ratio = false;
+
+    // straight line segment on equator
+    if (std::abs(c) < std::numeric_limits<float>::epsilon() && std::abs(a) < std::numeric_limits<float>::epsilon())
     {
-        nY = 0.f;
+        ratio = (q - b) / (d - b);
+    }
+    else
+    {
+        if (std::abs(c) < std::numeric_limits<float>::epsilon())
+        {
+            // swap start/end
+            std::swap(a, c);
+            std::swap(b, d);
+            inverse_ratio = true;
+        }
+
+        float nY = (d * p - c * q) / (a * d - b * c);
+        // discretize the result to coordinate precision. it's a hack!
+        if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
+        {
+            nY = 0.f;
+        }
+
+        // compute ratio
+        ratio = (p - nY * a) / c;
     }
 
-    // compute ratio
-    float ratio = (p - nY * a) / c;
     if (std::isnan(ratio))
     {
         ratio = (target_coordinate == point ? 1.f : 0.f);
@@ -200,6 +220,12 @@ FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &s
         ratio = 1.f;
     }
 
+    // we need to do this, if we switched start/end coordinates
+    if (inverse_ratio)
+    {
+        ratio = 1.0f - ratio;
+    }
+
     //compute the nearest location
     FixedPointCoordinate nearest_location;
     BOOST_ASSERT(!std::isnan(ratio));
@@ -216,6 +242,7 @@ FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &s
         nearest_location.lat = static_cast<int>(y2lat(p) * COORDINATE_PRECISION);
         nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
     }
+
     BOOST_ASSERT(nearest_location.isValid());
     return FixedPointCoordinate::ApproximateEuclideanDistance(point, nearest_location);
 }