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 const & finalPos) { @@ -17,17 +19,27 @@ void AStarRouter::SetFinalRoadPos(vector 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 t = m_entries.insert(ShortestPath(finalPos[i])); - ASSERT(t.second, ()); - m_queue.push(PossiblePath(&*t.first)); + pair 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 const & startPos, vector & route) { #if defined(DEBUG) @@ -36,87 +48,97 @@ void AStarRouter::CalculateRoute(vector const & startPos, vector startSet(startPos.begin(), startPos.end()); - set startFeatureSet; - for (vector::const_iterator it = startPos.begin(); it != startPos.end(); ++it) - startFeatureSet.insert(it->GetFeatureId()); + vector 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 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; - bool const bStartFeature = startFeatureSet.find(current->GetPos().GetFeatureId()) != startFeatureSet.end(); - if (bStartFeature && startSet.find(current->GetPos()) != startSet.end()) + /// @todo why do we even need this? + bool bStartFeature = binary_search(sortedStartFeatures.begin(), sortedStartFeatures.end(), v.GetPos().GetFeatureId()); + + 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 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; + 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; - 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(); - } - } - } + 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 const & goals) +double AStarRouter::HeuristicCostEstimate(Vertex const * v, vector 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 & route) const +void AStarRouter::ReconstructRoute(RoadPos const & destination, vector & 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; + double HeuristicCostEstimate(Vertex const * v, vector const & goals); + double DistanceBetween(Vertex const * v1, Vertex const * v2); + void ReconstructRoute(RoadPos const & destination, vector & route) const; - explicit PossiblePath(ShortestPath const * path, double fScore = 0.0) : m_path(path), m_fScore(fScore) {} + typedef priority_queue VertexQueueT; + VertexQueueT m_queue; - bool operator < (PossiblePath const & p) const - { - if (m_fScore != p.m_fScore) - return m_fScore > p.m_fScore; + typedef map RoadPosToDoubleMapT; + RoadPosToDoubleMapT m_bestDistance; - if (m_path->GetScoreG() != p.m_path->GetScoreG()) - return m_path->GetScoreG() > p.m_path->GetScoreG(); - - return !(m_path < p.m_path); - } - }; - - double HeuristicCostEstimate(ShortestPath const * s1, set const & goals); - double DistanceBetween(ShortestPath const * p1, ShortestPath const * p2); - void ReconstructRoute(ShortestPath const * path, vector & route) const; - - typedef priority_queue PossiblePathQueueT; - PossiblePathQueueT m_queue; - - typedef set ShortPathSetT; - ShortPathSetT m_entries; + typedef map 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 +using std::binary_search; using std::equal; using std::find; using std::find_if;