Add RoadGraphMockSource implementation for routing algorithm test

routing/road_graph.cpp

@@ -1 +1,16 @@
 #include "road_graph.hpp"
+
+#include "../std/sstream.hpp"
+
+
+namespace routing
+{
+
+string DebugPrint(RoadPos const & r)
+{
+  ostringstream ss;
+  ss << "{" << r.GetFeatureId() << ", " << r.m_pointId << ", " << r.IsForward() << "}";
+  return ss.str();
+}
+
+} // namespace routing

routing/road_graph.hpp

@@ -3,6 +3,7 @@
 #include "../geometry/point2d.hpp"
 
 #include "../base/assert.hpp"
+#include "../base/string_utils.hpp"
 
 #include "../std/vector.hpp"
 
@@ -14,6 +15,7 @@ namespace routing
 class RoadPos
 {
 public:
+  RoadPos() : m_featureId(0), m_pointId(0) {}
   RoadPos(uint32_t featureId, bool bForward, size_t pointId)
     : m_featureId((featureId << 1) + (bForward ? 1 : 0)), m_pointId(pointId)
   {
@@ -25,7 +27,14 @@ public:
   bool IsForward() const { return (m_featureId & 1) != 0; }
   uint32_t GetPointId() const { return m_pointId; }
 
+  bool operator==(RoadPos const & r) const
+  {
+    return (m_featureId == r.m_featureId && m_pointId == r.m_pointId);
+  }
+
 private:
+  friend string DebugPrint(RoadPos const & r);
+
   uint32_t m_featureId;
   uint32_t m_pointId;
 };
@@ -41,12 +50,28 @@ struct PossibleTurn
   /// Distance and time to get to this turn on old road.
   double m_metersCovered;
   double m_secondsCovered;
+
+  PossibleTurn() : m_metersCovered(0.0), m_secondsCovered(0.0) {}
 };
 
+inline string DebugPrint(PossibleTurn const & r)
+{
+  return DebugPrint(r.m_pos);
+}
+
 class IRoadGraph
 {
 public:
-  virtual void GetPossibleTurns(RoadPos const & pos, vector<PossibleTurn> & turns) = 0;
+  typedef vector<PossibleTurn> TurnsVectorT;
+  typedef vector<RoadPos> RoadPosVectorT;
+  typedef vector<m2::PointD> PointsVectorT;
+
+  /// Find all line feature sections, that follow the to pos section.
+  virtual void GetPossibleTurns(RoadPos const & pos, TurnsVectorT & turns) = 0;
+  /// Calculate distance in meters between two RoadPos that placed on the same feature
+  virtual double GetFeatureDistance(RoadPos const & p1, RoadPos const & p2) = 0;
+  /// Construct full path by road positions
+  virtual void ReconstructPath(RoadPosVectorT const & positions, PointsVectorT & poly) = 0;
 };
 
-}
+} // namespace routing

routing/routing_tests/road_graph_builder.cpp

@@ -0,0 +1,107 @@
+#include "road_graph_builder.hpp"
+
+#include "../../std/algorithm.hpp"
+
+using namespace routing;
+
+namespace routing_test
+{
+
+void RoadInfo::Swap(RoadInfo & r)
+{
+  m_points.swap(r.m_points);
+  std::swap(m_speedMS, r.m_speedMS);
+  std::swap(m_bothSides, r.m_bothSides);
+}
+
+void RoadGraphMockSource::AddRoad(RoadInfo & rd)
+{
+  /// @todo Do ASSERT for RoadInfo params.
+
+  uint32_t const id = m_roads.size();
+
+  // Count of sections! (not points)
+  size_t count = rd.m_points.size();
+  ASSERT_GREATER(count, 1, ());
+
+  for (size_t i = 0; i < count; ++i)
+  {
+    TurnsMapT::iterator j = m_turns.insert(make_pair(rd.m_points[i], TurnsVectorT())).first;
+
+    PossibleTurn t;
+    t.m_startPoint = rd.m_points.front();
+    t.m_endPoint = rd.m_points.back();
+    t.m_speed = rd.m_speedMS;
+
+    if (i > 0)
+    {
+      t.m_pos = RoadPos(id, true, i - 1);
+      j->second.push_back(t);
+    }
+
+    if (rd.m_bothSides && (i < count - 1))
+    {
+      t.m_pos = RoadPos(id, false, i);
+      j->second.push_back(t);
+    }
+  }
+
+  m_roads.push_back(RoadInfo());
+  m_roads.back().Swap(rd);
+}
+
+void RoadGraphMockSource::GetPossibleTurns(RoadPos const & pos, TurnsVectorT & turns)
+{
+  uint32_t const fID = pos.GetFeatureId();
+  ASSERT_LESS(fID, m_roads.size(), ());
+
+  vector<m2::PointD> const & points = m_roads[fID].m_points;
+
+  int const inc = pos.IsForward() ? -1 : 1;
+  int startID = pos.GetPointId();
+  int const count = static_cast<int>(points.size());
+
+  if (!pos.IsForward())
+    ++startID;
+
+  double const speed = m_roads[fID].m_speedMS;
+
+  double distance = 0.0;
+  double time = 0.0;
+  for (int i = startID; i >= 0 && i < count; i += inc)
+  {
+    double const len = points[i - inc].Length(points[i]);
+    distance += len;
+    time += len / speed;
+
+    TurnsMapT::const_iterator j = m_turns.find(points[i]);
+    if (j != m_turns.end())
+    {
+      vector<PossibleTurn> const & vec = j->second;
+      for (size_t k = 0; k < vec.size(); ++k)
+      {
+        if (vec[k].m_pos.GetFeatureId() != pos.GetFeatureId() ||
+            vec[k].m_pos.IsForward() == pos.IsForward())
+        {
+          PossibleTurn t = vec[k];
+
+          t.m_metersCovered = distance;
+          t.m_secondsCovered = time;
+          turns.push_back(t);
+        }
+      }
+    }
+  }
+}
+
+double RoadGraphMockSource::GetFeatureDistance(RoadPos const & p1, RoadPos const & p2)
+{
+  return 0.0;
+}
+
+void RoadGraphMockSource::ReconstructPath(RoadPosVectorT const & positions, PointsVectorT & poly)
+{
+
+}
+
+} // namespace routing_test

routing/routing_tests/road_graph_builder.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include "../road_graph.hpp"
+
+namespace routing_test
+{
+
+struct RoadInfo
+{
+  vector<m2::PointD> m_points;
+  double m_speedMS;
+  bool m_bothSides;
+
+  RoadInfo() : m_speedMS(0.0), m_bothSides(false) {}
+  void Swap(RoadInfo & r);
+};
+
+inline void swap(RoadInfo & r1, RoadInfo & r2)
+{
+  r1.Swap(r2);
+}
+
+class RoadGraphMockSource : public routing::IRoadGraph
+{
+  vector<RoadInfo> m_roads;
+
+  struct LessPoint
+  {
+    bool operator() (m2::PointD const & p1, m2::PointD const & p2) const
+    {
+      double const eps = 1.0E-6;
+
+      if (p1.x + eps < p2.x)
+        return true;
+      else if (p1.x > p2.x + eps)
+        return false;
+
+      return (p1.y + eps < p2.y);
+    }
+  };
+
+  typedef vector<routing::PossibleTurn> TurnsVectorT;
+  typedef map<m2::PointD, TurnsVectorT, LessPoint> TurnsMapT;
+  TurnsMapT m_turns;
+
+public:
+  void AddRoad(RoadInfo & rd);
+
+  virtual void GetPossibleTurns(routing::RoadPos const & pos, TurnsVectorT & turns);
+  virtual double GetFeatureDistance(routing::RoadPos const & p1, routing::RoadPos const & p2);
+  virtual void ReconstructPath(RoadPosVectorT const & positions, PointsVectorT & poly);
+};
+
+} // namespace routing_test

routing/routing_tests/road_graph_builder_test.cpp

@@ -0,0 +1,272 @@
+#include "../../testing/testing.hpp"
+
+#include "road_graph_builder.hpp"
+
+#include "../../base/logging.hpp"
+
+
+using namespace routing;
+using namespace routing_test;
+
+
+namespace
+{
+
+class EqualPos
+{
+  RoadPos m_pos;
+  double m_distance;
+public:
+  EqualPos(RoadPos const & pos, double d) : m_pos(pos), m_distance(d) {}
+  bool operator() (PossibleTurn const & r) const { return (r.m_pos == m_pos && r.m_metersCovered == m_distance); }
+};
+
+bool test_result(IRoadGraph::TurnsVectorT const & vec, RoadPos const & pos, double d)
+{
+  return find_if(vec.begin(), vec.end(), EqualPos(pos, d)) != vec.end();
+}
+
+}
+
+
+UNIT_TEST(RG_Builder_Smoke)
+{
+  RoadGraphMockSource src;
+
+  {
+    RoadInfo ri;
+    ri.m_bothSides = true;
+    ri.m_speedMS = 40;
+    ri.m_points.push_back(m2::PointD(0, 0));
+    ri.m_points.push_back(m2::PointD(5, 0));
+    ri.m_points.push_back(m2::PointD(10, 0));
+    ri.m_points.push_back(m2::PointD(15, 0));
+    ri.m_points.push_back(m2::PointD(20, 0));
+    src.AddRoad(ri);
+  }
+
+  {
+    RoadInfo ri;
+    ri.m_bothSides = true;
+    ri.m_speedMS = 40;
+    ri.m_points.push_back(m2::PointD(10, -10));
+    ri.m_points.push_back(m2::PointD(10, -5));
+    ri.m_points.push_back(m2::PointD(10, 0));
+    ri.m_points.push_back(m2::PointD(10, 5));
+    ri.m_points.push_back(m2::PointD(10, 10));
+    src.AddRoad(ri);
+  }
+
+  {
+    RoadInfo ri;
+    ri.m_bothSides = true;
+    ri.m_speedMS = 40;
+    ri.m_points.push_back(m2::PointD(15, -5));
+    ri.m_points.push_back(m2::PointD(15, 0));
+    src.AddRoad(ri);
+  }
+
+  {
+    RoadInfo ri;
+    ri.m_bothSides = true;
+    ri.m_speedMS = 40;
+    ri.m_points.push_back(m2::PointD(20, 0));
+    ri.m_points.push_back(m2::PointD(25, 5));
+    ri.m_points.push_back(m2::PointD(15, 5));
+    ri.m_points.push_back(m2::PointD(20, 0));
+    src.AddRoad(ri);
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    src.GetPossibleTurns(RoadPos(0, true, 1), vec);
+    TEST_EQUAL(vec.size(), 1, ());
+    TEST_EQUAL(vec[0].m_pos, RoadPos(0, true, 0), ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    src.GetPossibleTurns(RoadPos(0, false, 1), vec);
+    TEST_EQUAL(vec.size(), 7, ());
+    TEST(test_result(vec, RoadPos(0, false, 2), 5), ());
+    TEST(test_result(vec, RoadPos(0, false, 3), 10), ());
+    TEST(test_result(vec, RoadPos(1, true, 1), 5), ());
+    TEST(test_result(vec, RoadPos(1, false, 2), 5), ());
+    TEST(test_result(vec, RoadPos(2, true, 0), 10), ());
+    TEST(test_result(vec, RoadPos(3, false, 0), 15), ());
+    TEST(test_result(vec, RoadPos(3, true, 2), 15), ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    src.GetPossibleTurns(RoadPos(1, true, 0), vec);
+    TEST_EQUAL(vec.size(), 0, ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    src.GetPossibleTurns(RoadPos(1, false, 0), vec);
+    TEST_EQUAL(vec.size(), 5, ());
+    TEST(test_result(vec, RoadPos(1, false, 1), 5), ());
+    TEST(test_result(vec, RoadPos(1, false, 2), 10), ());
+    TEST(test_result(vec, RoadPos(1, false, 3), 15), ());
+    TEST(test_result(vec, RoadPos(0, true, 1), 10), ());
+    TEST(test_result(vec, RoadPos(0, false, 2), 10), ());
+  }
+
+  RoadInfo ri0;
+  ri0.m_bothSides = true;
+  ri0.m_speedMS = 40;
+  ri0.m_points.push_back(m2::PointD(0, 0));
+  ri0.m_points.push_back(m2::PointD(10, 0));
+  ri0.m_points.push_back(m2::PointD(25, 0));
+  ri0.m_points.push_back(m2::PointD(35, 0));
+  ri0.m_points.push_back(m2::PointD(70, 0));
+  ri0.m_points.push_back(m2::PointD(80, 0));
+
+  RoadInfo ri1;
+  ri1.m_bothSides = false;
+  ri1.m_speedMS = 20;
+  ri1.m_points.push_back(m2::PointD(0, 0));
+  ri1.m_points.push_back(m2::PointD(5, 10));
+  ri1.m_points.push_back(m2::PointD(5, 40));
+
+  RoadInfo ri2;
+  ri2.m_bothSides = false;
+  ri2.m_speedMS = 20;
+  ri2.m_points.push_back(m2::PointD(12, 25));
+  ri2.m_points.push_back(m2::PointD(10, 10));
+  ri2.m_points.push_back(m2::PointD(10, 0));
+
+  RoadInfo ri3;
+  ri3.m_bothSides = true;
+  ri3.m_speedMS = 30;
+  ri3.m_points.push_back(m2::PointD(5, 10));
+  ri3.m_points.push_back(m2::PointD(10, 10));
+  ri3.m_points.push_back(m2::PointD(70, 10));
+  ri3.m_points.push_back(m2::PointD(80, 10));
+
+  RoadInfo ri4;
+  ri4.m_bothSides = true;
+  ri4.m_speedMS = 20;
+  ri4.m_points.push_back(m2::PointD(25, 0));
+  ri4.m_points.push_back(m2::PointD(27, 25));
+
+  RoadInfo ri5;
+  ri5.m_bothSides = true;
+  ri5.m_speedMS = 30;
+  ri5.m_points.push_back(m2::PointD(35, 0));
+  ri5.m_points.push_back(m2::PointD(37, 30));
+  ri5.m_points.push_back(m2::PointD(70, 30));
+  ri5.m_points.push_back(m2::PointD(80, 30));
+
+  RoadInfo ri6;
+  ri6.m_bothSides = true;
+  ri6.m_speedMS = 20;
+  ri6.m_points.push_back(m2::PointD(70, 0));
+  ri6.m_points.push_back(m2::PointD(70, 10));
+  ri6.m_points.push_back(m2::PointD(70, 30));
+
+  RoadInfo ri7;
+  ri7.m_bothSides = true;
+  ri7.m_speedMS = 20;
+  ri7.m_points.push_back(m2::PointD(39, 55));
+  ri7.m_points.push_back(m2::PointD(80, 55));
+
+  RoadInfo ri8;
+  ri8.m_bothSides = false;
+  ri8.m_speedMS = 30;
+  ri8.m_points.push_back(m2::PointD(5, 40));
+  ri8.m_points.push_back(m2::PointD(18, 55));
+  ri8.m_points.push_back(m2::PointD(39, 55));
+  ri8.m_points.push_back(m2::PointD(37, 30));
+  ri8.m_points.push_back(m2::PointD(27, 25));
+  ri8.m_points.push_back(m2::PointD(12, 25));
+  ri8.m_points.push_back(m2::PointD(5, 40));
+
+  RoadGraphMockSource graph;
+  graph.AddRoad(ri0);
+  graph.AddRoad(ri1);
+  graph.AddRoad(ri2);
+  graph.AddRoad(ri3);
+  graph.AddRoad(ri4);
+  graph.AddRoad(ri5);
+  graph.AddRoad(ri6);
+  graph.AddRoad(ri7);
+  graph.AddRoad(ri8);
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    graph.GetPossibleTurns(RoadPos(0, false, 0), vec);
+    TEST_EQUAL(vec.size(), 8, ());
+    TEST(test_result(vec, RoadPos(0, false, 1), 10), ());
+    TEST(test_result(vec, RoadPos(0, false, 2), 25), ());
+    TEST(test_result(vec, RoadPos(0, false, 3), 35), ());
+    TEST(test_result(vec, RoadPos(0, false, 4), 70), ());
+    TEST(test_result(vec, RoadPos(2, true, 1), 10), ());
+    TEST(test_result(vec, RoadPos(5, false, 0), 35), ());
+    TEST(test_result(vec, RoadPos(6, false, 0), 70), ());
+    TEST(test_result(vec, RoadPos(4, false, 0), 25), ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    graph.GetPossibleTurns(RoadPos(8, true, 0), vec);
+    double const d = m2::PointD(18, 55).Length(m2::PointD(5, 40));
+    TEST_EQUAL(vec.size(), 2, ());
+    TEST(test_result(vec, RoadPos(1, true, 1), d), ());
+    TEST(test_result(vec, RoadPos(8, true, 5), d), ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    graph.GetPossibleTurns(RoadPos(2, true, 1), vec);
+    TEST_EQUAL(vec.size(), 4, ());
+    TEST(test_result(vec, RoadPos(3, true, 0), 10), ());
+    TEST(test_result(vec, RoadPos(3, false, 1), 10), ());
+    TEST(test_result(vec, RoadPos(2, true, 0), 10), ());
+    TEST(test_result(vec, RoadPos(8, true, 4), 10 + m2::PointD(10, 10).Length(m2::PointD(12, 25))), ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    graph.GetPossibleTurns(RoadPos(3, false, 0), vec);
+    TEST_EQUAL(vec.size(), 5, ());
+    TEST(test_result(vec, RoadPos(3, false, 1), 5), ());
+    TEST(test_result(vec, RoadPos(3, false, 2), 65), ());
+    TEST(test_result(vec, RoadPos(2, true, 0), 5), ());
+    TEST(test_result(vec, RoadPos(6, true, 0), 65), ());
+    TEST(test_result(vec, RoadPos(6, false, 1), 65), ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    graph.GetPossibleTurns(RoadPos(7, false, 0), vec);
+    TEST_EQUAL(vec.size(), 0, ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    graph.GetPossibleTurns(RoadPos(7, true, 0), vec);
+    TEST_EQUAL(vec.size(), 1, ());
+    TEST(test_result(vec, RoadPos(8, true, 1), 41), ());
+  }
+
+  {
+    IRoadGraph::TurnsVectorT vec;
+    graph.GetPossibleTurns(RoadPos(8, true, 3), vec);
+    double d = m2::PointD(27, 25).Length(m2::PointD(37, 30));
+    TEST_EQUAL(vec.size(), 8, ());
+    TEST(test_result(vec, RoadPos(8, true, 2), d), ());
+    TEST(test_result(vec, RoadPos(5, true, 0), d), ());
+    TEST(test_result(vec, RoadPos(5, false, 1), d), ());
+    d += m2::PointD(37, 30).Length(m2::PointD(39, 55));
+    TEST(test_result(vec, RoadPos(7, false, 0), d), ());;
+    TEST(test_result(vec, RoadPos(8, true, 1), d), ());
+    d += m2::PointD(18, 55).Length(m2::PointD(39, 55));
+    TEST(test_result(vec, RoadPos(8, true, 0), d), ());
+    d += m2::PointD(5, 40).Length(m2::PointD(18, 55));
+    TEST(test_result(vec, RoadPos(1, true, 1), d), ());
+    TEST(test_result(vec, RoadPos(8, true, 5), d), ());
+  }
+}

routing/routing_tests/routing_tests.pro

@@ -13,8 +13,11 @@ include($$ROOT_DIR/common.pri)
 SOURCES += \
   ../../testing/testingmain.cpp \
   routing_smoke.cpp \
+  road_graph_builder.cpp \
+  road_graph_builder_test.cpp \
 
-HEADERS +=
+HEADERS += \
+  road_graph_builder.hpp \