diff --git a/routing/dijkstra_router.cpp b/routing/dijkstra_router.cpp index 12b82266df..35d3914fea 100644 --- a/routing/dijkstra_router.cpp +++ b/routing/dijkstra_router.cpp @@ -1,89 +1,103 @@ #include "dijkstra_router.hpp" -#include "../base/assert.hpp" + +#include "../indexer/mercator.hpp" + #include "../base/logging.hpp" -#include "../std/set.hpp" + +#include "../std/algorithm.hpp" +#include "../std/map.hpp" +#include "../std/queue.hpp" namespace routing { +namespace +{ +template +void SortUnique(vector & v) +{ + sort(v.begin(), v.end()); + v.resize(unique(v.begin(), v.end()) - v.begin()); +} -DijkstraRouter::ShortestPath const * const DijkstraRouter::ShortestPath::FINAL_POS - = reinterpret_cast(1); +template +bool Contains(vector const & v, E const & e) +{ + return binary_search(v.begin(), v.end(), e); +} + +void ReconstructPath(RoadPos const & v, map const & parent, + vector & route) +{ + route.clear(); + RoadPos cur = v; + while (true) + { + route.push_back(cur); + auto it = parent.find(cur); + if (it == parent.end()) + break; + cur = it->second; + } +} + +} // namespace IRouter::ResultCode DijkstraRouter::CalculateRouteM2M(vector const & startPos, vector const & finalPos, vector & route) { - m_entries = PathSet(); - m_queue = PossiblePathQueue(); + priority_queue queue; - for (size_t i = 0; i < finalPos.size(); ++i) + // Upper bound on a distance from start positions to a position. + map dist; + + // Parent in a search tree. + map parent; + + vector sortedStartFeatures(startPos.size()); + for (size_t i = 0; i < startPos.size(); ++i) + sortedStartFeatures[i] = startPos[i].GetFeatureId(); + SortUnique(sortedStartFeatures); + + vector sortedStartPos(startPos.begin(), startPos.end()); + SortUnique(sortedStartPos); + + for (RoadPos const & fp : finalPos) + dist[fp] = 0.0; + for (auto const & p : dist) + queue.push(Vertex(p.first, 0.0 /* distance */)); + + while (!queue.empty()) { - pair t = m_entries.insert(ShortestPath(finalPos[i])); - ASSERT(t.second, ()); - m_queue.push(PossiblePath(0.0, &*t.first, ShortestPath::FINAL_POS)); - } - route.clear(); - set startSet(startPos.begin(), startPos.end()); - set startFeatureSet; - for (vector::const_iterator it = startPos.begin(); it != startPos.end(); ++it) - startFeatureSet.insert(it->GetFeatureId()); - while (!m_queue.empty()) - { - double const cost = m_queue.top().m_cost; - ShortestPath const * const pEntry = m_queue.top().m_pEntry; - ShortestPath const * const pParentEntry = m_queue.top().m_pParentEntry; - m_queue.pop(); - - LOG(LDEBUG, ("Visiting", pEntry->GetPos(), "with cost", cost)); - - if (pEntry->IsVisited()) - { - LOG(LDEBUG, ("Already visited before, skipping.")); + Vertex const v = queue.top(); + queue.pop(); + if (v.dist > dist[v.pos]) continue; - } - pEntry->SetParentAndMarkVisited(pParentEntry); -#ifdef DEBUG - if (pParentEntry == ShortestPath::FINAL_POS) + bool const isStartFeature = Contains(sortedStartFeatures, v.pos.GetFeatureId()); + if (isStartFeature && Contains(sortedStartPos, v.pos)) { - LOG(LDEBUG, ("Setting parent to", "FINAL_POS")); - } - else - { - LOG(LDEBUG, ("Setting parent to", pParentEntry->GetPos())); - } -#endif - - bool const bStartFeature = startFeatureSet.find(pEntry->GetPos().GetFeatureId()) != startFeatureSet.end(); - - if (bStartFeature && startSet.find(pEntry->GetPos()) != startSet.end()) - { - LOG(LDEBUG, ("Found result!")); - // Reached one of the starting points! - for (ShortestPath const * pE = pEntry; pE != ShortestPath::FINAL_POS; pE = pE->GetParentEntry()) - route.push_back(pE->GetPos()); - LOG(LDEBUG, (route)); + ReconstructPath(v.pos, parent, route); return IRouter::NoError; } - IRoadGraph::TurnsVectorT turns; - m_roadGraph->GetPossibleTurns(pEntry->GetPos(), turns, bStartFeature); - LOG(LDEBUG, ("Getting all turns", turns)); - for (IRoadGraph::TurnsVectorT::const_iterator iTurn = turns.begin(); iTurn != turns.end(); ++iTurn) + m_roadGraph->GetPossibleTurns(v.pos, turns, isStartFeature); + for (PossibleTurn const & turn : turns) { - PossibleTurn const & turn = *iTurn; - pair t = m_entries.insert(ShortestPath(turn.m_pos)); - if (t.second || !t.first->IsVisited()) + RoadPos const & pos = turn.m_pos; + if (v.pos == pos) + continue; + double const d = + v.dist + MercatorBounds::DistanceOnEarth(v.pos.GetSegEndpoint(), pos.GetSegEndpoint()); + auto it = dist.find(pos); + if (it == dist.end() || d < it->second) { - // We've either never pushed or never poped this road before. - double nextCost = cost + turn.m_secondsCovered; - m_queue.push(PossiblePath(nextCost, &*t.first, pEntry)); - LOG(LDEBUG, ("Pushing", t.first->GetPos(), "with cost", nextCost)); + dist[pos] = d; + parent[pos] = v.pos; + queue.push(Vertex(pos, d)); } } } return IRouter::RouteNotFound; } - - -} // namespace routing +} // namespace routing diff --git a/routing/dijkstra_router.hpp b/routing/dijkstra_router.hpp index 5714a3e639..097c3300a8 100644 --- a/routing/dijkstra_router.hpp +++ b/routing/dijkstra_router.hpp @@ -2,16 +2,10 @@ #include "road_graph_router.hpp" -#include "../geometry/point2d.hpp" - -#include "../std/queue.hpp" -#include "../std/set.hpp" -#include "../std/vector.hpp" - +#include "../std/map.hpp" namespace routing { - class DijkstraRouter : public RoadGraphRouter { typedef RoadGraphRouter BaseT; @@ -20,76 +14,21 @@ public: DijkstraRouter(Index const * pIndex = 0) : BaseT(pIndex) {} // IRouter overrides: - string GetName() const override { return "dijkstra-pedestrian"; } + string GetName() const override { return "pedestrian-dijkstra"; } // RoadGraphRouter overrides: ResultCode CalculateRouteM2M(vector const & startPos, vector const & finalPos, vector & route) override; private: - class ShortestPath + struct Vertex { - public: - explicit ShortestPath(RoadPos pos, ShortestPath const * pParentEntry = NULL) - : m_pos(pos), m_pParentEntry(pParentEntry) {} + Vertex(RoadPos const & pos, double dist) : pos(pos), dist(dist) {} - bool operator < (ShortestPath const & e) const - { - return m_pos < e.m_pos; - } + bool operator<(Vertex const & v) const { return dist > v.dist; } - RoadPos GetPos() const { return m_pos; } - ShortestPath const * GetParentEntry() const { return m_pParentEntry; } - bool IsVisited() const { return m_pParentEntry != NULL; } - void SetParentAndMarkVisited(ShortestPath const * pParentEntry) const { m_pParentEntry = pParentEntry; } - - static ShortestPath const * const FINAL_POS; - - private: - RoadPos m_pos; - mutable ShortestPath const * m_pParentEntry; + RoadPos pos; + double dist; }; - - struct PossiblePath - { - double m_cost; - ShortestPath const * m_pEntry; - ShortestPath const * m_pParentEntry; - - PossiblePath(double cost, ShortestPath const * pEntry, ShortestPath const * pParentEntry) - : m_cost(cost), m_pEntry(pEntry), m_pParentEntry(pParentEntry) {} - - bool operator < (PossiblePath const & p) const - { - if (m_cost != p.m_cost) - return m_cost > p.m_cost; - - // Comparisons below are used to make the algorithm stable. - if (m_pEntry->GetPos() < p.m_pEntry->GetPos()) - return true; - if (p.m_pEntry->GetPos() < m_pEntry->GetPos()) - return false; - if (!m_pParentEntry && p.m_pParentEntry) - return true; - if (!p.m_pParentEntry && m_pParentEntry) - return false; - if (!m_pParentEntry && !p.m_pParentEntry) - { - if (m_pParentEntry->GetPos() < p.m_pParentEntry->GetPos()) - return true; - if (p.m_pParentEntry->GetPos() < m_pParentEntry->GetPos()) - return false; - } - - return false; - } - }; - - typedef set PathSet; - PathSet m_entries; - - typedef priority_queue PossiblePathQueue; - PossiblePathQueue m_queue; }; - -} // namespace routing +} // namespace routing