From 24b01fae00a14e12ec0165cdad52395833c90da2 Mon Sep 17 00:00:00 2001
From: Moritz Kobitzsch <moritz@mapbox.com>
Date: Fri, 2 Dec 2016 22:11:04 +0100
Subject: [PATCH] fix extraction of coordinate along a way (#3397)

Also fix interpolation when coordinates are duplicated at the end of the vector
---
 .../guidance/coordinate_extractor.cpp         | 42 +++++++++++--------
 1 file changed, 24 insertions(+), 18 deletions(-)

diff --git a/src/extractor/guidance/coordinate_extractor.cpp b/src/extractor/guidance/coordinate_extractor.cpp
index 6dc9c9e02..96a2bc0dd 100644
--- a/src/extractor/guidance/coordinate_extractor.cpp
+++ b/src/extractor/guidance/coordinate_extractor.cpp
@@ -447,18 +447,17 @@ CoordinateExtractor::ExtractCoordinateAtLength(const double distance,
 
     // find the beginning fo the segment (begin here and above for the length cache need to match
     // up!)
-    const auto coordinate_base =
-        std::find_if(coordinates.begin() + 1, coordinates.end(), find_coordinate_at_distance) - 1;
+    const auto coordinate_after =
+        std::find_if(coordinates.begin() + 1, coordinates.end(), find_coordinate_at_distance);
 
-    if (static_cast<std::size_t>(std::distance(coordinates.begin(), coordinate_base) + 1) >=
-        coordinates.size())
+    if (coordinate_after == coordinates.end())
         return coordinates.back();
 
     const auto interpolation_factor =
         ComputeInterpolationFactor(distance - accumulated_distance, 0, *length_cache_itr);
 
     return util::coordinate_calculation::interpolateLinear(
-        interpolation_factor, *coordinate_base, *(coordinate_base + 1));
+        interpolation_factor, *std::prev(coordinate_after), *coordinate_after);
 }
 
 util::Coordinate CoordinateExtractor::ExtractCoordinateAtLength(
@@ -475,26 +474,29 @@ util::Coordinate CoordinateExtractor::ExtractCoordinateAtLength(
             util::coordinate_calculation::haversineDistance(last_coordinate, coordinate);
         const auto result = (accumulated_distance + segment_distance) >= distance;
         if (!result)
+        {
             accumulated_distance += segment_distance;
+            last_coordinate = coordinate;
+        }
 
         return result;
     };
 
     // find the begin of the segment containing the coordinate
-    const auto coordinate_base =
-        std::find_if(coordinates.begin() + 1, coordinates.end(), coordinate_at_distance) - 1;
+    const auto coordinate_after =
+        std::find_if(coordinates.begin() + 1, coordinates.end(), coordinate_at_distance);
 
-    if (static_cast<std::size_t>(std::distance(coordinates.begin(), coordinate_base) + 1) >=
-        coordinates.size())
+    if (coordinate_after == coordinates.end())
         return coordinates.back();
 
-    const auto interpolation_factor = ComputeInterpolationFactor(
-        distance - accumulated_distance,
-        0,
-        util::coordinate_calculation::haversineDistance(*coordinate_base, *(coordinate_base + 1)));
+    const auto interpolation_factor =
+        ComputeInterpolationFactor(distance - accumulated_distance,
+                                   0,
+                                   util::coordinate_calculation::haversineDistance(
+                                       *std::prev(coordinate_after), *coordinate_after));
 
     return util::coordinate_calculation::interpolateLinear(
-        interpolation_factor, *coordinate_base, *(coordinate_base + 1));
+        interpolation_factor, *std::prev(coordinate_after), *coordinate_after);
 }
 
 util::Coordinate CoordinateExtractor::GetCoordinateCloseToTurn(const NodeID from_node,
@@ -928,11 +930,15 @@ CoordinateExtractor::TrimCoordinatesToLength(std::vector<util::Coordinate> coord
         const auto distance_between_last_coordinates =
             util::coordinate_calculation::haversineDistance(*(coordinates.end() - 2),
                                                             *(coordinates.end() - 1));
-        const auto interpolation_factor =
-            ComputeInterpolationFactor(length_cache.back(), 0, distance_between_last_coordinates);
 
-        coordinates.back() = util::coordinate_calculation::interpolateLinear(
-            interpolation_factor, *(coordinates.end() - 2), coordinates.back());
+        if (distance_between_last_coordinates > 0)
+        {
+            const auto interpolation_factor = ComputeInterpolationFactor(
+                length_cache.back(), 0, distance_between_last_coordinates);
+
+            coordinates.back() = util::coordinate_calculation::interpolateLinear(
+                interpolation_factor, *(coordinates.end() - 2), coordinates.back());
+        }
         return coordinates;
     }
     else