[routing] Get current direction point enchancement.

4 files changed, 36 insertions, 6 deletions

diff --git a/routing/base/followed_polyline.cpp b/routing/base/followed_polyline.cpp
index 19d9ae9ee1..5ffda53d0c 100644
--- a/routing/base/followed_polyline.cpp
+++ b/routing/base/followed_polyline.cpp
@@ -151,4 +151,18 @@ double FollowedPolyline::GetMercatorDistanceFromBegin() const
 
   return distance;
 }
+
+void FollowedPolyline::GetCurrentDirectionPoint(m2::PointD & pt, double toleranceM) const
+{
+  ASSERT(IsValid(), ());
+  size_t currentIndex = min(m_current.m_ind + 1, m_poly.GetSize() - 1);
+  m2::PointD point = m_poly.GetPoint(currentIndex);
+  for (; currentIndex < m_poly.GetSize() - 1; point = m_poly.GetPoint(++currentIndex))
+  {
+    if (MercatorBounds::DistanceOnEarth(point, m_current.m_pt) > toleranceM)
+      break;
+  }
+
+  pt = point;
+}
 }  //  namespace routing
diff --git a/routing/base/followed_polyline.hpp b/routing/base/followed_polyline.hpp
index 911892fcfc..89fdb1d4e1 100644
--- a/routing/base/followed_polyline.hpp
+++ b/routing/base/followed_polyline.hpp
@@ -26,10 +26,11 @@ public:
 
   double GetMercatorDistanceFromBegin() const;
 
-  void GetCurrentDirectionPoint(m2::PointD & pt) const
-  {
-    pt = m_poly.GetPoint(min(m_current.m_ind + 1, m_poly.GetSize() - 1));
-  }
+  /*! \brief Return next navigation point for direction widgets.
+   *  Returns first geomety point from the polyline after your location if it is farther then
+   *  toleranceM.
+   */
+  void GetCurrentDirectionPoint(m2::PointD & pt, double toleranceM) const;
 
   struct Iter
   {
diff --git a/routing/route.cpp b/routing/route.cpp
index 82a96c6dca..c59b6248b3 100644
--- a/routing/route.cpp
+++ b/routing/route.cpp
@@ -173,9 +173,9 @@ void Route::GetCurrentTurn(double & distanceToTurnMeters, turns::TurnItem & turn
 void Route::GetCurrentDirectionPoint(m2::PointD & pt) const
 {
   if (m_routingSettings.m_keepPedestrianInfo)
-    m_simplifiedPoly.GetCurrentDirectionPoint(pt);
+    m_simplifiedPoly.GetCurrentDirectionPoint(pt, kOnEndToleranceM);
   else
-    m_poly.GetCurrentDirectionPoint(pt);
+    m_poly.GetCurrentDirectionPoint(pt, kOnEndToleranceM);
 }
 
 bool Route::MoveIterator(location::GpsInfo const & info) const
diff --git a/routing/routing_tests/followed_polyline_test.cpp b/routing/routing_tests/followed_polyline_test.cpp
index abb7da1858..d534584fb7 100644
--- a/routing/routing_tests/followed_polyline_test.cpp
+++ b/routing/routing_tests/followed_polyline_test.cpp
@@ -83,4 +83,19 @@ UNIT_TEST(FollowedPolylineDistanceCalculationTest)
                                                    kTestDirectedPolyline.Back());
   ASSERT_LESS(pow(distance - masterDistance, 2), 0.001, (distance, masterDistance));
 }
+
+UNIT_TEST(FollowdPolylineDirectionTest)
+{
+  m2::PolylineD testPolyline({{0, 0}, {1.00003, 0}, {1.00003, 1}});
+  FollowedPolyline polyline(testPolyline.Begin(), testPolyline.End());
+  TEST_EQUAL(polyline.GetCurrentIter().m_ind, 0, ());
+  m2::PointD directionPoint;
+  polyline.GetCurrentDirectionPoint(directionPoint, 20);
+  TEST_EQUAL(directionPoint, testPolyline.GetPoint(1), ());
+  polyline.UpdateProjection(MercatorBounds::RectByCenterXYAndSizeInMeters({1.0, 0}, 2));
+  polyline.GetCurrentDirectionPoint(directionPoint, 0.0001);
+  TEST_EQUAL(directionPoint, testPolyline.GetPoint(1), ());
+  polyline.GetCurrentDirectionPoint(directionPoint, 20);
+  TEST_EQUAL(directionPoint, testPolyline.GetPoint(2), ());
+}
 }  // namespace routing_test