[pedestrian] Rewrite of the A* algorithm.

5 files changed, 108 insertions, 103 deletions

diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp
index 0119ef0dc3..1b864c76c6 100644
--- a/routing/astar_router.cpp
+++ b/routing/astar_router.cpp
@@ -1,13 +1,15 @@
 #include "astar_router.hpp"
 
 #include "../base/logging.hpp"
+#include "../base/macros.hpp"
 #include "../geometry/distance_on_sphere.hpp"
 #include "../indexer/mercator.hpp"
+#include "../std/algorithm.hpp"
 
 namespace routing
 {
 
-static double const MAX_SPEED = 36.11111111111; // m/s
+static double const kMaxSpeed = 5000.0 / 3600; // m/s
 
 void AStarRouter::SetFinalRoadPos(vector<RoadPos> const & finalPos)
 {
@@ -17,17 +19,27 @@ void AStarRouter::SetFinalRoadPos(vector<RoadPos> const & finalPos)
 #endif  // defined(DEBUG)
 
   ASSERT_GREATER(finalPos.size(), 0, ());
-  m_entries.clear();
-  PossiblePathQueueT().swap(m_queue);
-  for (size_t i = 0; i < finalPos.size(); ++i)
+  m_bestDistance.clear();
+  for (auto const & roadPos : finalPos)
   {
-    pair<ShortPathSetT::iterator, bool> t = m_entries.insert(ShortestPath(finalPos[i]));
-    ASSERT(t.second, ());
-    m_queue.push(PossiblePath(&*t.first));
+    pair<RoadPosToDoubleMapT::iterator, bool> t = m_bestDistance.insert({roadPos, 0.0});
+    UNUSED_VALUE(t);
+    ASSERT_EQUAL(t.second, true, ());
   }
-
 }
 
+// This implementation is based on the view that the A* algorithm
+// is equivalent to Dijkstra's algorithm that is run on a reweighted
+// version of the graph. If an edge (v, w) has length l(v, w), its reduced
+// cost is l_r(v, w) = l(v, w) + pi(w) - pi(v), where pi() is any function
+// that ensures l_r(v, w) >= 0 for every edge. We set pi() to calculate
+// the shortest possible distance to a goal node, and this is a common
+// heuristic that people use in A*.
+// Refer to this paper for more information:
+// http://research.microsoft.com/pubs/154937/soda05.pdf
+//
+// The vertices of the graph are of type RoadPos.
+// The edges of the graph are of type PossibleTurn.
 void AStarRouter::CalculateRoute(vector<RoadPos> const & startPos, vector<RoadPos> & route)
 {
 #if defined(DEBUG)
@@ -36,87 +48,97 @@ void AStarRouter::CalculateRoute(vector<RoadPos> const & startPos, vector<RoadPo
 #endif // defined(DEBUG)
 
   route.clear();
-  set<RoadPos> startSet(startPos.begin(), startPos.end());
-  set<uint32_t> startFeatureSet;
-  for (vector<RoadPos>::const_iterator it = startPos.begin(); it != startPos.end(); ++it)
-    startFeatureSet.insert(it->GetFeatureId());
+  vector<uint32_t> sortedStartFeatures(startPos.size());
+  for (size_t i = 0; i < startPos.size(); ++i)
+    sortedStartFeatures[i] = startPos[i].GetFeatureId();
+  sort(sortedStartFeatures.begin(), sortedStartFeatures.end());
+  sortedStartFeatures.resize(unique(sortedStartFeatures.begin(), sortedStartFeatures.end()) - sortedStartFeatures.begin());
+
+  vector<RoadPos> sortedStartPos(startPos.begin(), startPos.end());
+  sort(sortedStartPos.begin(), sortedStartPos.end());
+
+  VertexQueueT().swap(m_queue);
+  for (RoadPosToDoubleMapT::const_iterator it = m_bestDistance.begin(); it != m_bestDistance.end(); ++it)
+    m_queue.push(Vertex(it->first));
 
   while (!m_queue.empty())
   {
-    PossiblePath const & currPath = m_queue.top();
-    double const fScore = currPath.m_fScore;
-    ShortestPath const * current = currPath.m_path;
-    current->RemovedFromOpenSet();
+    Vertex const v = m_queue.top();
     m_queue.pop();
 
-    current->SetVisited();
+    if (v.GetReducedDist() > m_bestDistance[v.GetPos()])
+      continue;
+
+    /// @todo why do we even need this?
+    bool bStartFeature = binary_search(sortedStartFeatures.begin(), sortedStartFeatures.end(), v.GetPos().GetFeatureId());
 
-    bool const bStartFeature = startFeatureSet.find(current->GetPos().GetFeatureId()) != startFeatureSet.end();
-    if (bStartFeature && startSet.find(current->GetPos()) != startSet.end())
+    if (bStartFeature && binary_search(sortedStartPos.begin(), sortedStartPos.end(), v.GetPos()))
     {
-      ReconstructRoute(current, route);
+      ReconstructRoute(v.GetPos(), route);
       return;
     }
 
     IRoadGraph::TurnsVectorT turns;
-    m_pRoadGraph->GetPossibleTurns(current->GetPos(), turns, bStartFeature);
+    m_pRoadGraph->GetPossibleTurns(v.GetPos(), turns, bStartFeature);
     for (IRoadGraph::TurnsVectorT::const_iterator it = turns.begin(); it != turns.end(); ++it)
     {
       PossibleTurn const & turn = *it;
-      pair<ShortPathSetT::iterator, bool> t = m_entries.insert(ShortestPath(turn.m_pos, current));
-      if (t.second || !t.first->IsVisited())
-      {
-        ShortestPath const * nextPath = &*t.first;
-        ASSERT_GREATER(turn.m_speed, 0.0, ());
-        double nextScoreG = fScore + turn.m_secondsCovered;//DistanceBetween(current, nextPath) / turn.m_speed;
-
-        if (!nextPath->IsInOpenSet() || nextScoreG < nextPath->GetScoreG())
-        {
-          nextPath->SetParent(current);
-          nextPath->SetScoreG(nextScoreG);
-          if (!nextPath->IsInOpenSet())
-          {
-            m_queue.push(PossiblePath(nextPath, nextScoreG + HeuristicCostEstimate(nextPath, startSet)));
-            nextPath->AppendedIntoOpenSet();
-          }
-        }
-      }
+      ASSERT_GREATER(turn.m_speed, 0.0, ()); /// @todo why?
+      Vertex const w = Vertex(turn.m_pos);
+      if (v.GetPos() == w.GetPos())
+        continue;
+      double len = DistanceBetween(&v, &w) / kMaxSpeed;
+      double piV = HeuristicCostEstimate(&v, sortedStartPos);
+      double piW = HeuristicCostEstimate(&w, sortedStartPos);
+      double newReducedDist = v.GetReducedDist() + len + piW - piV;
+
+      RoadPosToDoubleMapT::const_iterator t = m_bestDistance.find(turn.m_pos);
+      if (t != m_bestDistance.end() && newReducedDist >= t->second)
+        continue;
+
+      w.SetReducedDist(newReducedDist);
+      m_bestDistance[w.GetPos()] = newReducedDist;
+      RoadPosParentMapT::iterator pit = m_parent.find(w.GetPos());
+      m_parent[w.GetPos()] = v.GetPos();
+      m_queue.push(w);
     }
+
   }
 
   LOG(LDEBUG, ("No route found!"));
   // Route not found.
 }
 
-double AStarRouter::HeuristicCostEstimate(AStarRouter::ShortestPath const * s1, set<RoadPos> const & goals)
+double AStarRouter::HeuristicCostEstimate(Vertex const * v, vector<RoadPos> const & goals)
 {
-  /// @todo support of more than one goals
+  // @todo support of more than one goal
   ASSERT_GREATER(goals.size(), 0, ());
 
-  m2::PointD const & b = (*goals.begin()).GetSegEndpoint();
-  m2::PointD const & e = s1->GetPos().GetSegEndpoint();
+  m2::PointD const & b = goals[0].GetSegEndpoint();
+  m2::PointD const & e = v->GetPos().GetSegEndpoint();
 
-  return MercatorBounds::DistanceOnEarth(b, e) / MAX_SPEED;
+  return MercatorBounds::DistanceOnEarth(b, e) / kMaxSpeed;
 }
 
-double AStarRouter::DistanceBetween(ShortestPath const * p1, ShortestPath const * p2)
+double AStarRouter::DistanceBetween(Vertex const * v1, Vertex const * v2)
 {
-  m2::PointD const & b = p1->GetPos().GetSegEndpoint();
-  m2::PointD const & e = p2->GetPos().GetSegEndpoint();
+  m2::PointD const & b = v1->GetPos().GetSegEndpoint();
+  m2::PointD const & e = v2->GetPos().GetSegEndpoint();
   return MercatorBounds::DistanceOnEarth(b, e);
 }
 
-void AStarRouter::ReconstructRoute(ShortestPath const * path, vector<RoadPos> & route) const
+void AStarRouter::ReconstructRoute(RoadPos const & destination, vector<RoadPos> & route) const
 {
-  ShortestPath const * p = path;
+  RoadPos rp = destination;
 
-  while (p)
+  LOG(LDEBUG, ("A-star has found a route"));
+  while (true)
   {
-    route.push_back(p->GetPos());
-    if (p->GetParentEntry())
-      p = p->GetParentEntry();
-    else
-      p = 0;
+    route.push_back(rp);
+    RoadPosParentMapT::const_iterator it = m_parent.find(rp);
+    if (it == m_parent.end())
+      break;
+    rp = it->second;
   }
 }
 
diff --git a/routing/astar_router.hpp b/routing/astar_router.hpp
index fa2724ae6c..feaa889127 100644
--- a/routing/astar_router.hpp
+++ b/routing/astar_router.hpp
@@ -2,6 +2,7 @@
 
 #include "road_graph_router.hpp"
 #include "../std/queue.hpp"
+#include "../std/map.hpp"
 
 namespace routing
 {
@@ -18,71 +19,47 @@ public:
 
 protected:
 
-  class ShortestPath
+  // Vertex is what is going to be put in the priority queue
+  class Vertex
   {
   public:
-    explicit ShortestPath(RoadPos pos, ShortestPath const * pParentEntry = NULL, double gScore = 0.0)
-      : m_pos(pos), m_pParentEntry(pParentEntry), m_gScore(gScore), m_isInOpenSet(false), m_isVisited(false) {}
+    explicit Vertex(RoadPos pos, Vertex const * parent = NULL, double dist = 0.0)
+      : m_pos(pos), m_parent(parent), m_reducedDist(dist) {}
 
-    bool operator < (ShortestPath const & e) const
+    bool operator < (Vertex const & v) const
     {
-      return m_pos < e.m_pos;
+      return m_reducedDist > v.m_reducedDist;
     }
 
     RoadPos GetPos() const { return m_pos; }
-    ShortestPath const * GetParentEntry() const { return m_pParentEntry; }
-    bool IsVisited() const { return m_isVisited; }
-    void SetParent(ShortestPath const * pParentEntry) const
+    inline void SetParent(Vertex const * parent) const
     {
-      ASSERT_NOT_EQUAL(this, pParentEntry, ());
-      m_pParentEntry = pParentEntry;
+      ASSERT_NOT_EQUAL(this, parent, ());
+      m_parent = parent;
     }
+    inline Vertex const * GetParent() const { return m_parent; }
 
-    void SetVisited() const { m_isVisited = true; }
-
-    void AppendedIntoOpenSet() const { m_isInOpenSet = true; }
-    void RemovedFromOpenSet() const { m_isInOpenSet = false; }
-    bool IsInOpenSet() const { return m_isInOpenSet; }
-
-    inline void SetScoreG(double g) const { m_gScore = g; }
-    inline double GetScoreG() const { return m_gScore; }
+    inline void SetReducedDist(double dist) const { m_reducedDist = dist; }
+    inline double GetReducedDist() const { return m_reducedDist; }
 
   private:
     RoadPos m_pos;
-    mutable ShortestPath const * m_pParentEntry;
-    mutable double m_gScore;
-    mutable bool m_isInOpenSet;
-    mutable bool m_isVisited;
+    mutable Vertex const * m_parent;
+    mutable double m_reducedDist;
   };
 
-  struct PossiblePath
-  {
-    ShortestPath const * m_path;
-    double m_fScore;
-
-    explicit PossiblePath(ShortestPath const * path, double fScore = 0.0) : m_path(path), m_fScore(fScore) {}
-
-    bool operator < (PossiblePath const & p) const
-    {
-      if (m_fScore != p.m_fScore)
-        return m_fScore > p.m_fScore;
-
-      if (m_path->GetScoreG() != p.m_path->GetScoreG())
-        return m_path->GetScoreG() > p.m_path->GetScoreG();
-
-      return !(m_path < p.m_path);
-    }
-  };
+  double HeuristicCostEstimate(Vertex const * v, vector<RoadPos> const & goals);
+  double DistanceBetween(Vertex const * v1, Vertex const * v2);
+  void ReconstructRoute(RoadPos const & destination, vector<RoadPos> & route) const;
 
-  double HeuristicCostEstimate(ShortestPath const * s1, set<RoadPos> const & goals);
-  double DistanceBetween(ShortestPath const * p1, ShortestPath const * p2);
-  void ReconstructRoute(ShortestPath const * path, vector<RoadPos> & route) const;
+  typedef priority_queue<Vertex> VertexQueueT;
+  VertexQueueT m_queue;
 
-  typedef priority_queue<PossiblePath> PossiblePathQueueT;
-  PossiblePathQueueT m_queue;
+  typedef map<RoadPos, double> RoadPosToDoubleMapT;
+  RoadPosToDoubleMapT m_bestDistance;
 
-  typedef set<ShortestPath> ShortPathSetT;
-  ShortPathSetT m_entries;
+  typedef map<RoadPos, RoadPos> RoadPosParentMapT;
+  RoadPosParentMapT m_parent;
 };
 
 
diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp
index eb708b95b8..00acef2257 100644
--- a/routing/features_road_graph.cpp
+++ b/routing/features_road_graph.cpp
@@ -285,7 +285,7 @@ void FeaturesRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route
 
       ptID += inc;
 
-    } while (!m2::AlmostEqual(pt, lastPt));
+    } while (ptID >= 0 && ptID < ft1.GetPointsCount() && !m2::AlmostEqual(pt, lastPt));
 
     // Assign current processing feature.
     if (diffIDs)
diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp
index c14b35ebd3..67f0a01789 100644
--- a/routing/road_graph.hpp
+++ b/routing/road_graph.hpp
@@ -30,6 +30,11 @@ public:
     return (m_featureId == r.m_featureId && m_segId == r.m_segId);
   }
 
+  bool operator!=(RoadPos const & r) const
+  {
+    return (m_featureId != r.m_featureId || m_segId != r.m_segId);
+  }
+
   bool operator<(RoadPos const & r) const
   {
     if (m_featureId != r.m_featureId)
diff --git a/std/algorithm.hpp b/std/algorithm.hpp
index 761271018b..d918197c37 100644
--- a/std/algorithm.hpp
+++ b/std/algorithm.hpp
@@ -7,6 +7,7 @@
 
 #include <algorithm>
 
+using std::binary_search;
 using std::equal;
 using std::find;
 using std::find_if;