[routing] Dijkstra router.

routing/dijkstra_router.cpp

@@ -0,0 +1,90 @@
+#include "dijkstra_router.hpp"
+#include "../base/assert.hpp"
+#include "../base/logging.hpp"
+#include "../std/set.hpp"
+
+namespace routing
+{
+
+DijkstraRouter::ShortestPath const * const DijkstraRouter::ShortestPath::FINAL_POS
+  = reinterpret_cast<ShortestPath const *>(1);
+
+void DijkstraRouter::SetFinalRoadPos(vector<RoadPos> const & finalPos)
+{
+  m_entries = PathSet();
+  m_queue = PossiblePathQueue();
+
+  for (size_t i = 0; i < finalPos.size(); ++i)
+  {
+    pair<PathSet::iterator, bool> t = m_entries.insert(ShortestPath(finalPos[i]));
+    ASSERT(t.second, ());
+    m_queue.push(PossiblePath(0.0, &*t.first, ShortestPath::FINAL_POS));
+  }
+}
+
+void DijkstraRouter::CalculateRoute(vector<RoadPos> const & startPos, vector<RoadPos> & route)
+{
+  route.clear();
+  set<RoadPos> startSet(startPos.begin(), startPos.end());
+  while (!m_queue.empty())
+  {
+    double cost = m_queue.top().m_cost;
+    ShortestPath const * pEntry = m_queue.top().m_pEntry;
+    ShortestPath 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."));
+      continue;
+    }
+    pEntry->SetParentAndMarkVisited(pParentEntry);
+
+#ifdef DEBUG
+    if (pParentEntry == ShortestPath::FINAL_POS)
+    {
+      LOG(LDEBUG, ("Setting parent to", "FINAL_POS"));
+    }
+    else
+    {
+      LOG(LDEBUG, ("Setting parent to", pParentEntry->GetPos()));
+    }
+#endif
+
+    if (startSet.find(pEntry->GetPos()) != startSet.end())
+    {
+      LOG(LDEBUG, ("Found result!"));
+      // Reached one of the starting points!
+      while (pEntry != ShortestPath::FINAL_POS)
+      {
+        route.push_back(pEntry->GetPos());
+        pEntry = pEntry->GetParentEntry();
+      }
+      return;
+    }
+
+    IRoadGraph::TurnsVectorT turns;
+    m_pRoadGraph->GetPossibleTurns(pEntry->GetPos(), turns);
+    LOG(LDEBUG, ("Getting all turns", turns));
+    for (IRoadGraph::TurnsVectorT::const_iterator iTurn = turns.begin(); iTurn != turns.end(); ++iTurn)
+    {
+      PossibleTurn const & turn = *iTurn;
+      pair<PathSet::iterator, bool> t = m_entries.insert(ShortestPath(turn.m_pos));
+      if (t.second || !t.first->IsVisited())
+      {
+        // 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));
+      }
+    }
+  }
+
+  LOG(LDEBUG, ("No debug found!"));
+  // Route not found.
+}
+
+
+} // namespace routing

routing/dijkstra_router.hpp

@@ -0,0 +1,86 @@
+#pragma once
+
+#include "road_graph_router.hpp"
+
+#include "../geometry/point2d.hpp"
+#include "../std/queue.hpp"
+#include "../std/set.hpp"
+#include "../std/vector.hpp"
+
+namespace routing
+{
+
+class DijkstraRouter : public RoadGraphRouter
+{
+public:
+  virtual string GetName() const { return "dijkstra"; }
+  virtual void SetFinalRoadPos(vector<RoadPos> const & finalPos);
+  virtual void CalculateRoute(vector<RoadPos> const & startPos, vector<RoadPos> & route);
+
+private:
+  class ShortestPath
+  {
+  public:
+    explicit ShortestPath(RoadPos pos, ShortestPath const * pParentEntry = NULL)
+      : m_pos(pos), m_pParentEntry(pParentEntry) {}
+
+    bool operator < (ShortestPath const & e) const
+    {
+      return m_pos < e.m_pos;
+    }
+
+    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;
+  };
+
+  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<ShortestPath> PathSet;
+  PathSet m_entries;
+
+  typedef priority_queue<PossiblePath> PossiblePathQueue;
+  PossiblePathQueue m_queue;
+};
+
+} // namespace routing

routing/road_graph.cpp

@@ -9,7 +9,7 @@ namespace routing
 string DebugPrint(RoadPos const & r)
 {
   ostringstream ss;
-  ss << "{" << r.GetFeatureId() << ", " << r.m_pointId << ", " << r.IsForward() << "}";
+  ss << "{" << r.GetFeatureId() << ", " << r.IsForward() << ", " << r.m_pointId << "}";
   return ss.str();
 }
 

routing/road_graph.hpp

@@ -32,6 +32,15 @@ public:
     return (m_featureId == r.m_featureId && m_pointId == r.m_pointId);
   }
 
+  bool operator < (RoadPos const & r) const
+  {
+    if (m_featureId != r.m_featureId)
+      return m_featureId < r.m_featureId;
+    if (m_pointId != r.m_pointId)
+      return m_pointId < r.m_pointId;
+    return false;
+  }
+
 private:
   friend string DebugPrint(RoadPos const & r);
 

routing/road_graph_router.cpp

@@ -0,0 +1,24 @@
+#include "road_graph_router.hpp"
+#include "route.hpp"
+
+namespace routing
+{
+
+void RoadGraphRouter::SetFinalPoint(m2::PointD const & finalPt)
+{
+  // TODO: Calculate finalPos.
+  vector<RoadPos> finalPos;
+  this->SetFinalRoadPos(finalPos);
+}
+
+void RoadGraphRouter::CalculateRoute(m2::PointD const & startPt, ReadyCallback const & callback)
+{
+  // TODO: Calculate startPos.
+  vector<RoadPos> startPos;
+  vector<RoadPos> route;
+  this->CalculateRoute(startPos, route);
+  // TODO: Call back the callback
+}
+
+} // namespace routing
+

routing/road_graph_router.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "router.hpp"
+#include "road_graph.hpp"
+
+#include "../geometry/point2d.hpp"
+#include "../std/vector.hpp"
+
+namespace routing
+{
+
+class IRoadGraph;
+
+class RoadGraphRouter : public IRouter
+{
+public:
+  virtual void SetFinalPoint(m2::PointD const & finalPt);
+  virtual void CalculateRoute(m2::PointD const & startPt, ReadyCallback const & callback);
+
+  virtual void SetFinalRoadPos(vector<RoadPos> const & finalPos) = 0;
+  virtual void CalculateRoute(vector<RoadPos> const & startPos, vector<RoadPos> & route) = 0;
+  virtual void SetRoadGraph(IRoadGraph * pRoadGraph) { m_pRoadGraph = pRoadGraph; }
+
+protected:
+  IRoadGraph * m_pRoadGraph;
+};
+
+} // namespace routing

routing/routing.pro

@@ -15,6 +15,8 @@ SOURCES += \
     road_graph.cpp \
     helicopter_router.cpp \
     osrm_router.cpp \
+    road_graph_router.cpp \
+    dijkstra_router.cpp \
 
 HEADERS += \
     route.hpp \
@@ -23,3 +25,5 @@ HEADERS += \
     road_graph.hpp \
     helicopter_router.hpp \
     osrm_router.hpp \
+    road_graph_router.hpp \
+    dijkstra_router.hpp \

routing/routing_tests/dijkstra_router_test.cpp

@@ -0,0 +1,51 @@
+#include "../../testing/testing.hpp"
+#include "../dijkstra_router.hpp"
+#include "road_graph_builder.hpp"
+#include "../../base/logging.hpp"
+#include "../../base/macros.hpp"
+
+using namespace routing;
+using namespace routing_test;
+
+template <size_t finalPosSize, size_t startPosSize, size_t expectedSize>
+void TestDijkstraRouter(RoadPos (&finalPos)[finalPosSize],
+                        RoadPos (&startPos)[startPosSize],
+                        RoadPos (&expected)[expectedSize])
+{
+  RoadGraphMockSource graph;
+  InitRoadGraphMockSourceWithTest2(graph);
+  DijkstraRouter router;
+  router.SetRoadGraph(&graph);
+  router.SetFinalRoadPos(vector<RoadPos>(&finalPos[0], &finalPos[0] + ARRAY_SIZE(finalPos)));
+  vector<RoadPos> result;
+  router.CalculateRoute(vector<RoadPos>(&startPos[0], &startPos[0] + ARRAY_SIZE(startPos)), result);
+  TEST_EQUAL(vector<RoadPos>(&expected[0], &expected[0] + ARRAY_SIZE(expected)), result, ());
+}
+
+UNIT_TEST(Dijkstra_Router_City_Simple)
+{
+  RoadPos finalPos[] = { RoadPos(7, true, 0) };
+  RoadPos startPos[] = { RoadPos(1, true, 0) };
+  RoadPos expected[] = { RoadPos(1, true, 0),
+                         RoadPos(1, true, 1),
+                         RoadPos(8, true, 0),
+                         RoadPos(8, true, 1),
+                         RoadPos(7, true, 0) };
+  TestDijkstraRouter(finalPos, startPos, expected);
+}
+
+UNIT_TEST(Dijkstra_Router_City_ReallyFunnyLoop)
+{
+  // my::g_LogLevel = LDEBUG;
+
+  RoadPos finalPos[] = { RoadPos(1, true, 0) };
+  RoadPos startPos[] = { RoadPos(1, true, 1) };
+  RoadPos expected[] = { RoadPos(1, true, 1),
+                         RoadPos(8, true, 1),
+                         RoadPos(8, true, 2),
+                         RoadPos(5, false, 0),
+                         RoadPos(0, false, 1),
+                         RoadPos(0, false, 0),
+                         RoadPos(1, true, 0) };
+  TestDijkstraRouter(finalPos, startPos, expected);
+}

routing/routing_tests/routing_tests.pro

@@ -15,6 +15,7 @@ SOURCES += \
   routing_smoke.cpp \
   road_graph_builder.cpp \
   road_graph_builder_test.cpp \
+  dijkstra_router_test.cpp \
 
 HEADERS += \
   road_graph_builder.hpp \