Taking into account vecor of user moving for choosing the best start edge.

4 files changed, 72 insertions, 20 deletions

diff --git a/routing/index_router.cpp b/routing/index_router.cpp
index 5aed3acab0..6f9bad72f9 100644
--- a/routing/index_router.cpp
+++ b/routing/index_router.cpp
@@ -222,7 +222,7 @@ IRouter::ResultCode IndexRouter::CalculateRoute(Checkpoints const & checkpoints,
 }
 
 IRouter::ResultCode IndexRouter::DoCalculateRoute(Checkpoints const & checkpoints,
-                                                  m2::PointD const & /* startDirection */,
+                                                  m2::PointD const & startDirection,
                                                   RouterDelegate const & delegate, Route & route)
 {
   m_lastRoute.reset();
@@ -253,7 +253,7 @@ IRouter::ResultCode IndexRouter::DoCalculateRoute(Checkpoints const & checkpoint
   segments.push_back(IndexGraphStarter::kStartFakeSegment);
 
   Segment startSegment;
-  if (!FindClosestSegment(checkpoints.GetPointFrom(), true /* isOutgoing */, graph, startSegment))
+  if (!FindBestSegment(checkpoints.GetPointFrom(), startDirection, true, graph, startSegment))
     return IRouter::StartPointNotFound;
   
   size_t subrouteSegmentsBegin = 0;
@@ -332,7 +332,7 @@ IRouter::ResultCode IndexRouter::CalculateSubroute(Checkpoints const & checkpoin
   auto const & finishCheckpoint = checkpoints.GetPoint(subrouteIdx + 1);
 
   Segment finishSegment;
-  if (!FindClosestSegment(finishCheckpoint, false /* isOutgoing */, graph, finishSegment))
+  if (!FindBestSegment(finishCheckpoint, {0.0, 0.0}, false, graph, finishSegment))
   {
     bool const isLastSubroute = subrouteIdx == checkpoints.GetNumSubroutes() - 1;
     return isLastSubroute ? IRouter::EndPointNotFound : IRouter::IntermediatePointNotFound;
@@ -399,7 +399,7 @@ IRouter::ResultCode IndexRouter::AdjustRoute(Checkpoints const & checkpoints,
 
   Segment startSegment;
   m2::PointD const & pointFrom = checkpoints.GetPointFrom();
-  if (!FindClosestSegment(pointFrom, true /* isOutgoing */, graph, startSegment))
+  if (!FindBestSegment(pointFrom, startDirection, true, graph, startSegment))
     return IRouter::StartPointNotFound;
 
   auto const & lastSubroutes = m_lastRoute->GetSubroutes();
@@ -499,8 +499,9 @@ WorldGraph IndexRouter::MakeWorldGraph()
   return graph;
 }
 
-bool IndexRouter::FindClosestSegment(m2::PointD const & point, bool isOutgoing,
-                                     WorldGraph & worldGraph, Segment & closestSegment) const
+bool IndexRouter::FindBestSegment(m2::PointD const & point, m2::PointD const & startDirection,
+                                  bool isOutgoing, WorldGraph & worldGraph,
+                                  Segment & bestSegment) const
 {
   auto const file = platform::CountryFile(m_countryFileFn(point));
   MwmSet::MwmHandle handle = m_index.GetMwmHandleByCountryFile(file);
@@ -513,9 +514,14 @@ bool IndexRouter::FindClosestSegment(m2::PointD const & point, bool isOutgoing,
   vector<pair<Edge, Junction>> candidates;
   m_roadGraph.FindClosestEdges(point, kMaxRoadCandidates, candidates);
 
-  double minDistance = numeric_limits<double>::max();
+  // Minimum distance to neighbouring segments.
+  double minDist = numeric_limits<double>::max();
   size_t minIndex = candidates.size();
 
+  // Minimum distance to neighbouring segments which are parallel to |startDirection|.
+  double minDistForParallel = numeric_limits<double>::max();
+  size_t minIndexForParallel = candidates.size();
+
   for (size_t i = 0; i < candidates.size(); ++i)
   {
     Edge const & edge = candidates[i].first;
@@ -524,22 +530,58 @@ bool IndexRouter::FindClosestSegment(m2::PointD const & point, bool isOutgoing,
     if (edge.GetFeatureId().m_mwmId != mwmId || IsDeadEnd(segment, isOutgoing, worldGraph))
       continue;
 
+    // Distance without taking into account angle.
     m2::DistanceToLineSquare<m2::PointD> squaredDistance;
     squaredDistance.SetBounds(edge.GetStartJunction().GetPoint(), edge.GetEndJunction().GetPoint());
-    double const distance = squaredDistance(point);
-    if (distance < minDistance)
-    {
-      minDistance = distance;
-      minIndex = i;
-    }
+    double const dist = squaredDistance(point);
+    auto const UpdateMinValuesIfNeeded = [&](double & minDistToUpdate, size_t & minIndexToUpdate) {
+      if (minDistToUpdate > dist)
+      {
+        minDistToUpdate = dist;
+        minIndexToUpdate = i;
+      }
+    };
+
+    UpdateMinValuesIfNeeded(minDist, minIndex);
+
+    // Distance for almost parallel vectors.
+    if (startDirection.IsAlmostZero())
+      continue;
+
+    m2::PointD const candidateDirection =
+        edge.GetEndJunction().GetPoint() - edge.GetStartJunction().GetPoint();
+    if (candidateDirection.IsAlmostZero())
+      continue;
+
+    double const cosAng =
+        m2::DotProduct(startDirection.Normalize(), candidateDirection.Normalize());
+    // Let us consider that vectors are almost parallel if cos of angle between them is more
+    // than |kMinCosAngForAlmostParallelVectors|.
+    // Note. If cosinus of an angle is more 0.97. The angle should be more than 14 degrees.
+    double constexpr kMinCosAngForAlmostParallelVectors = 0.97;
+    if (cosAng < kMinCosAngForAlmostParallelVectors)
+      continue;
+
+    // Vectors of |edge| and gps heading are almost parallel.
+    UpdateMinValuesIfNeeded(minDistForParallel, minIndexForParallel);
   }
 
   if (minIndex == candidates.size())
+  {
+    CHECK_EQUAL(minIndexForParallel, candidates.size(), ());
     return false;
+  }
 
-  Edge const & closestEdge = candidates[minIndex].first;
-  closestSegment = Segment(m_numMwmIds->GetId(file), closestEdge.GetFeatureId().m_index,
-                           closestEdge.GetSegId(), true /* forward */);
+  // Note. The idea behind choosing |bestEdge| is
+  // * trying to find closest to |point| edge (segment) which is almost parallel to
+  // |startDirection|;
+  // * if there's no such edge (segment) trying to find the segment which is closest the |point|.
+  // * if anyway nothing is found - returns false.
+  Edge const & bestEdge =
+      (minIndexForParallel == candidates.size() ? candidates[minIndex].first
+                                                : candidates[minIndexForParallel].first);
+  bestSegment = Segment(m_numMwmIds->GetId(file), bestEdge.GetFeatureId().m_index,
+                        bestEdge.GetSegId(), true /* forward */);
   return true;
 }
 
diff --git a/routing/index_router.hpp b/routing/index_router.hpp
index 0419480548..4e7a0f5b1e 100644
--- a/routing/index_router.hpp
+++ b/routing/index_router.hpp
@@ -65,11 +65,16 @@ private:
 
   WorldGraph MakeWorldGraph();
 
-  /// \brief Finds closest edges which may be considered as start of finish of the route.
+  /// \brief Finds best edges which may be considered as the start of the finish of the route.
+  /// If it's possible |bestSegment| will be filled with a segment which is almost parallel
+  /// to |startDirection|.
   /// \param isOutgoing == true is |point| is considered as the start of the route.
   /// isOutgoing == false is |point| is considered as the finish of the route.
-  bool FindClosestSegment(m2::PointD const & point, bool isOutgoing, WorldGraph & worldGraph,
-                          Segment & closestSegment) const;
+  bool FindBestSegment(m2::PointD const & point,
+                       m2::PointD const & startDirection,
+                       bool isOutgoing,
+                       WorldGraph & worldGraph,
+                       Segment & bestSegment) const;
   // Input route may contains 'leaps': shortcut edges from mwm border enter to exit.
   // ProcessLeaps replaces each leap with calculated route through mwm.
   IRouter::ResultCode ProcessLeaps(vector<Segment> const & input,
diff --git a/routing/routing_session.cpp b/routing/routing_session.cpp
index 8344d1fceb..5c752759f6 100644
--- a/routing/routing_session.cpp
+++ b/routing/routing_session.cpp
@@ -94,7 +94,7 @@ void RoutingSession::RebuildRoute(m2::PointD const & startPoint,
   m_checkpoints.SetPointFrom(startPoint);
   // Use old-style callback construction, because lambda constructs buggy function on Android
   // (callback param isn't captured by value).
-  m_router->CalculateRoute(m_checkpoints, startPoint - m_lastGoodPosition, adjustToPrevRoute,
+  m_router->CalculateRoute(m_checkpoints, m_currentDirection, adjustToPrevRoute,
                            DoReadyCallback(*this, readyCallback, m_routingSessionMutex),
                            m_progressCallback, timeoutSec);
 }
@@ -234,6 +234,9 @@ RoutingSession::State RoutingSession::OnLocationPositionChanged(GpsInfo const &
       }
     }
 
+    if (m_lastGoodPosition != m2::PointD::Zero())
+      m_currentDirection = m_userCurrentPosition - m_lastGoodPosition;
+
     m_lastGoodPosition = m_userCurrentPosition;
   }
   else
diff --git a/routing/routing_session.hpp b/routing/routing_session.hpp
index 940bb0c50f..5595cfd5a6 100644
--- a/routing/routing_session.hpp
+++ b/routing/routing_session.hpp
@@ -223,6 +223,8 @@ private:
   double m_lastDistance;
   int m_moveAwayCounter;
   m2::PointD m_lastGoodPosition;
+  // |m_currentDirection| is a vector from the position before last good position to last good position.
+  m2::PointD m_currentDirection;
   m2::PointD m_userCurrentPosition;
 
   // Sound turn notification parameters.