[routing] Road graph on mwm features.

indexer/feature.hpp

@@ -190,6 +190,18 @@ public:
     }
   }
 
+  inline size_t GetPointsCount() const
+  {
+    ASSERT(m_bPointsParsed, ());
+    return m_Points.size();
+  }
+  inline m2::PointD const & GetPoint(size_t i) const
+  {
+    ASSERT_LESS(i, m_Points.size(), ());
+    ASSERT(m_bPointsParsed, ());
+    return m_Points[i];
+  }
+
   template <typename FunctorT>
   void ForEachPoint(FunctorT f, int scale) const
   {

routing/features_road_graph.cpp

@@ -0,0 +1,224 @@
+#include "features_road_graph.hpp"
+
+#include "../indexer/index.hpp"
+#include "../indexer/classificator.hpp"
+#include "../indexer/feature_data.hpp"
+
+#include "../geometry/distance_on_sphere.hpp"
+
+#include "../base/logging.hpp"
+
+namespace routing
+{
+
+uint32_t const READ_ROAD_SCALE = 13;
+double const READ_CROSS_RADIUS = 10.0;
+double const DEFAULT_SPEED_MS = 15.0;
+
+
+FeatureRoadGraph::FeatureRoadGraph(Index * pIndex, size_t mwmID)
+  : m_pIndex(pIndex), m_mwmID(mwmID)
+{
+  m_onewayType = classif().GetTypeByPath(vector<string>(1, "oneway"));
+}
+
+uint32_t FeatureRoadGraph::GetStreetReadScale()
+{
+  return READ_ROAD_SCALE;
+}
+
+class CrossFeaturesLoader
+{
+  uint32_t m_featureID;
+  FeatureRoadGraph & m_graph;
+  m2::PointD m_point;
+  IRoadGraph::TurnsVectorT & m_turns;
+  size_t m_count;
+
+public:
+  CrossFeaturesLoader(uint32_t fID, FeatureRoadGraph & graph,
+                      m2::PointD const & pt, IRoadGraph::TurnsVectorT & turns)
+    : m_featureID(fID), m_graph(graph), m_point(pt), m_turns(turns), m_count(0)
+  {
+  }
+
+  size_t GetCount() const
+  {
+    return m_count;
+  }
+
+  void operator()(FeatureType const & ft)
+  {
+    FeatureID fID = ft.GetID();
+    if (m_featureID == fID.m_offset)
+      return;
+
+    feature::TypesHolder types(ft);
+    if (!m_graph.IsStreet(types))
+      return;
+
+    ft.ParseGeometry(FeatureType::BEST_GEOMETRY);
+
+    bool const isOneWay = m_graph.IsOneway(types);
+    size_t const count = ft.GetPointsCount();
+
+    PossibleTurn t;
+    t.m_startPoint = ft.GetPoint(0);
+    t.m_endPoint = ft.GetPoint(count - 1);
+
+    for (size_t i = 0; i < count; ++i)
+    {
+      m2::PointD const & p = ft.GetPoint(i);
+
+      /// @todo Is this a correct way to compare?
+      if (!m2::AlmostEqual(m_point, p))
+        continue;
+
+      if (i > 0)
+      {
+        ++m_count;
+        t.m_pos = RoadPos(fID.m_offset, true, i - 1);
+        m_turns.push_back(t);
+      }
+
+      if (!isOneWay && (i < count - 1))
+      {
+        ++m_count;
+        t.m_pos = RoadPos(fID.m_offset, false, i);
+        m_turns.push_back(t);
+      }
+    }
+
+  }
+};
+
+double CalcDistanceMeters(m2::PointD const & p1, m2::PointD const & p2)
+{
+  return ms::DistanceOnEarth(MercatorBounds::YToLat(p1.y), MercatorBounds::XToLon(p1.x),
+                             MercatorBounds::YToLat(p2.y), MercatorBounds::XToLon(p2.x));
+}
+
+void FeatureRoadGraph::GetPossibleTurns(RoadPos const & pos, vector<PossibleTurn> & turns)
+{
+  Index::FeaturesLoaderGuard loader(*m_pIndex, m_mwmID);
+
+  uint32_t const ftId = pos.GetFeatureId();
+  FeatureType ft;
+  loader.GetFeature(ftId, ft);
+  ft.ParseGeometry(FeatureType::BEST_GEOMETRY);
+
+  int const count = static_cast<int>(ft.GetPointsCount());
+  bool const isForward = pos.IsForward();
+  bool const isOneWay = IsOneway(ft);
+  int const inc = isForward ? -1 : 1;
+
+  int startID = pos.GetPointId();
+  ASSERT_LESS(startID, count, ());
+  if (!isForward)
+    ++startID;
+
+  PossibleTurn thisTurn;
+  thisTurn.m_startPoint = ft.GetPoint(0);
+  thisTurn.m_endPoint = ft.GetPoint(count - 1);
+
+  double distance = 0.0;
+  double time = 0.0;
+  for (int i = startID; i >= 0 && i < count; i += inc)
+  {
+    if (i != startID)
+    {
+      distance += CalcDistanceMeters(ft.GetPoint(i), ft.GetPoint(i - inc));
+      time += distance / DEFAULT_SPEED_MS;
+    }
+
+    m2::PointD const & pt = ft.GetPoint(i);
+
+    // Find possible turns to point[i] from other features.
+    size_t const last = turns.size();
+    CrossFeaturesLoader crossLoader(ftId, *this, pt, turns);
+    m_pIndex->ForEachInRect(crossLoader,
+                            MercatorBounds::RectByCenterXYAndSizeInMeters(pt, READ_CROSS_RADIUS),
+                            READ_ROAD_SCALE);
+
+    // Skip if there are no turns on point
+    if (crossLoader.GetCount() > 0)
+    {
+      // Push turn points for this feature.
+      if (isForward)
+      {
+        if (i > 0)
+        {
+          thisTurn.m_pos = RoadPos(ftId, true, i - 1);
+          turns.push_back(thisTurn);
+        }
+      }
+      else
+      {
+        if (!isOneWay && (i != count - 1))
+        {
+          thisTurn.m_pos = RoadPos(ftId, false, i);
+          turns.push_back(thisTurn);
+        }
+      }
+    }
+
+    // Update distance and time information.
+    for (size_t j = last; j < turns.size(); ++j)
+    {
+      turns[j].m_metersCovered = distance;
+      turns[j].m_secondsCovered = time;
+      turns[j].m_speed = DEFAULT_SPEED_MS;
+    }
+  }
+
+  // Check cycle
+  if (m2::AlmostEqual(ft.GetPoint(0), ft.GetPoint(count - 1)))
+  {
+    /// @todo calculate distance and speed
+    if (isForward)
+    {
+      double distance = 0;
+      for (int i = pos.GetPointId(); i > 0; --i)
+        distance += CalcDistanceMeters(ft.GetPoint(i), ft.GetPoint(i - 1));
+
+      thisTurn.m_pos = RoadPos(ftId, true, count - 2);
+      thisTurn.m_metersCovered = distance;
+      thisTurn.m_secondsCovered = distance / DEFAULT_SPEED_MS;
+      turns.push_back(thisTurn);
+    }
+    else if (!isOneWay)
+    {
+      double distance = 0;
+      for (size_t i = pos.GetPointId(); i < count - 1; ++i)
+        distance += CalcDistanceMeters(ft.GetPoint(i), ft.GetPoint(i + 1));
+
+      thisTurn.m_pos = RoadPos(ftId, false, 0);
+      thisTurn.m_metersCovered = distance;
+      thisTurn.m_secondsCovered = distance / DEFAULT_SPEED_MS;
+      turns.push_back(thisTurn);
+    }
+  }
+}
+
+double FeatureRoadGraph::GetFeatureDistance(RoadPos const & p1, RoadPos const & p2)
+{
+  /// @todo Implement distance calculation
+  return 0.0;
+}
+
+void FeatureRoadGraph::ReconstructPath(RoadPosVectorT const & positions, PointsVectorT & poly)
+{
+  /// @todo Implement path reconstruction
+}
+
+bool FeatureRoadGraph::IsStreet(feature::TypesHolder const & types) const
+{
+  return m_checker(types);
+}
+
+bool FeatureRoadGraph::IsOneway(feature::TypesHolder const & types) const
+{
+  return types.Has(m_onewayType);
+}
+
+}

routing/features_road_graph.hpp

@@ -0,0 +1,43 @@
+#pragma once
+#include "road_graph.hpp"
+
+#include "../search/ftypes_matcher.hpp"
+
+
+
+class Index;
+
+namespace feature
+{
+  class TypesHolder;
+}
+
+namespace routing
+{
+
+class FeatureRoadGraph : public IRoadGraph
+{
+public:
+  FeatureRoadGraph(Index * pIndex, size_t mwmID);
+
+  virtual void GetPossibleTurns(RoadPos const & pos, vector<PossibleTurn> & turns);
+  virtual double GetFeatureDistance(RoadPos const & p1, RoadPos const & p2);
+  virtual void ReconstructPath(RoadPosVectorT const & positions, PointsVectorT & poly);
+
+  static uint32_t GetStreetReadScale();
+
+private:
+  friend class CrossFeaturesLoader;
+
+  bool IsStreet(feature::TypesHolder const & types) const;
+  bool IsOneway(feature::TypesHolder const & types) const;
+
+private:
+  ftypes::IsStreetChecker m_checker;
+  uint32_t m_onewayType;
+
+  Index * m_pIndex;
+  size_t m_mwmID;
+};
+
+}

routing/routing.pro

@@ -17,6 +17,7 @@ SOURCES += \
     osrm_router.cpp \
     road_graph_router.cpp \
     dijkstra_router.cpp \
+    features_road_graph.cpp \
 
 HEADERS += \
     route.hpp \
@@ -27,3 +28,4 @@ HEADERS += \
     osrm_router.hpp \
     road_graph_router.hpp \
     dijkstra_router.hpp \
+    features_road_graph.hpp \

routing/routing_tests/features_road_graph_test.cpp

@@ -0,0 +1,233 @@
+#include "../../testing/testing.hpp"
+
+#include "../../base/logging.hpp"
+
+#include "../../indexer/classificator_loader.hpp"
+#include "../../indexer/index.hpp"
+#include "../../indexer/feature.hpp"
+
+#include "../../search/ftypes_matcher.hpp"
+
+#include "../features_road_graph.hpp"
+
+using namespace routing;
+
+
+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;
+  }
+};
+
+class Name2IdMapping
+{
+  map<string, uint32_t> m_name2Id;
+  map<uint32_t, string> m_id2Name;
+  ftypes::IsStreetChecker m_checker;
+
+public:
+  void operator()(FeatureType const & ft)
+  {
+    if (!m_checker(ft)) return;
+
+    string name;
+    bool hasName = ft.GetName(0, name);
+    ASSERT(hasName, ());
+
+    m_name2Id[name] = ft.GetID().m_offset;
+    m_id2Name[ft.GetID().m_offset] = name;
+  }
+
+  uint32_t GetId(string const & name)
+  {
+    ASSERT(m_name2Id.find(name) != m_name2Id.end(), ());
+    return m_name2Id[name];
+  }
+
+  string const & GetName(uint32_t id)
+  {
+    ASSERT(m_id2Name.find(id) != m_id2Name.end(), ());
+    return m_id2Name[id];
+  }
+};
+
+bool TestResult(IRoadGraph::TurnsVectorT const & vec, RoadPos const & pos, double d)
+{
+  return find_if(vec.begin(), vec.end(), EqualPos(pos, d)) != vec.end();
+}
+
+void FeatureID2Name(IRoadGraph::TurnsVectorT & vec, Name2IdMapping & mapping)
+{
+  for (size_t i = 0; i < vec.size(); ++i)
+  {
+    PossibleTurn & t = vec[i];
+    string name = mapping.GetName(t.m_pos.GetFeatureId());
+    int id = 0;
+    bool isInt = strings::to_int(name, id);
+    ASSERT(isInt, ());
+
+    t.m_pos = RoadPos(static_cast<uint32_t>(id), t.m_pos.IsForward(), t.m_pos.GetPointId());
+  }
+}
+
+}
+
+
+UNIT_TEST(FRG_Smoke)
+{
+  classificator::Load();
+
+  // ----- test 1 -----
+  {
+    Index index;
+    m2::RectD rect;
+    if (!index.Add("route_test1.mwm", rect))
+    {
+      LOG(LERROR, ("MWM file not found"));
+      return;
+    }
+
+    FeatureRoadGraph graph(&index, 0);
+    Name2IdMapping mapping;
+    index.ForEachInRect(mapping, MercatorBounds::FullRect(), graph.GetStreetReadScale());
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("0"), true, 1), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 0, ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("0"), false, 1), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 7, ());
+      TEST(TestResult(vec, RoadPos(0, false, 2), 5), ());
+      TEST(TestResult(vec, RoadPos(0, false, 3), 10), ());
+      TEST(TestResult(vec, RoadPos(1, true, 1), 5), ());
+      TEST(TestResult(vec, RoadPos(1, false, 2), 5), ());
+      TEST(TestResult(vec, RoadPos(2, true, 0), 10), ());
+      TEST(TestResult(vec, RoadPos(3, false, 0), 15), ());
+      TEST(TestResult(vec, RoadPos(3, true, 2), 15), ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("1"), true, 0), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 0, ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("1"), false, 0), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 3, ());
+      TEST(TestResult(vec, RoadPos(1, false, 2), 10), ());
+      TEST(TestResult(vec, RoadPos(0, true, 1), 10), ());
+      TEST(TestResult(vec, RoadPos(0, false, 2), 10), ());
+    }
+
+  }
+
+  // ----- test 2 -----
+  {
+    Index index;
+    m2::RectD rect;
+    if (!index.Add("route_test2.mwm", rect))
+    {
+      LOG(LERROR, ("MWM file not found"));
+      return;
+    }
+
+    FeatureRoadGraph graph(&index, 0);
+    Name2IdMapping mapping;
+    index.ForEachInRect(mapping, MercatorBounds::FullRect(), graph.GetStreetReadScale());
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("0"), false, 0), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 8, ());
+      TEST(TestResult(vec, RoadPos(0, false, 1), -1), ());
+      TEST(TestResult(vec, RoadPos(0, false, 2), -1), ());
+      TEST(TestResult(vec, RoadPos(0, false, 3), -1), ());
+      TEST(TestResult(vec, RoadPos(0, false, 4), -1), ());
+      TEST(TestResult(vec, RoadPos(2, true, 1), -1), ());
+      TEST(TestResult(vec, RoadPos(5, false, 0), -1), ());
+      TEST(TestResult(vec, RoadPos(6, false, 0), -1), ());
+      TEST(TestResult(vec, RoadPos(4, false, 0), -1), ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("8"), true, 0), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 2, ());
+      TEST(TestResult(vec, RoadPos(1, true, 1), -1), ());
+      TEST(TestResult(vec, RoadPos(8, true, 5), -1), ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("2"), true, 1), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 4, ());
+      TEST(TestResult(vec, RoadPos(3, true, 0), -1), ());
+      TEST(TestResult(vec, RoadPos(3, false, 1), -1), ());
+      TEST(TestResult(vec, RoadPos(2, true, 0), -1), ());
+      TEST(TestResult(vec, RoadPos(8, true, 4), -1), ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("3"), false, 0), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 5, ());
+      TEST(TestResult(vec, RoadPos(3, false, 1), -1), ());
+      TEST(TestResult(vec, RoadPos(3, false, 2), -1), ());
+      TEST(TestResult(vec, RoadPos(2, true, 0), -1), ());
+      TEST(TestResult(vec, RoadPos(6, true, 0), -1), ());
+      TEST(TestResult(vec, RoadPos(6, false, 1), -1), ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("7"), false, 0), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 0, ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("7"), true, 0), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 1, ());
+      TEST(TestResult(vec, RoadPos(8, true, 1), -1), ());
+    }
+
+    {
+      IRoadGraph::TurnsVectorT vec;
+      graph.GetPossibleTurns(RoadPos(mapping.GetId("8"), true, 3), vec);
+      FeatureID2Name(vec, mapping);
+      TEST_EQUAL(vec.size(), 7, ());
+      TEST(TestResult(vec, RoadPos(8, true, 2), -1), ());
+      TEST(TestResult(vec, RoadPos(5, true, 0), -1), ());
+      TEST(TestResult(vec, RoadPos(5, false, 1), -1), ());
+      TEST(TestResult(vec, RoadPos(7, false, 0), -1), ());
+      TEST(TestResult(vec, RoadPos(8, true, 1), -1), ());
+      TEST(TestResult(vec, RoadPos(1, true, 1), -1), ());
+      TEST(TestResult(vec, RoadPos(8, true, 5), -1), ());
+    }
+  }
+}

routing/routing_tests/routing_tests.pro

@@ -6,12 +6,15 @@ CONFIG -= app_bundle
 TEMPLATE = app
 
 ROOT_DIR = ../..
-DEPENDENCIES = routing platform indexer geometry coding base
+DEPENDENCIES = routing search platform indexer geometry coding base protobuf
+
+macx-*: LIBS *= "-framework Foundation" "-framework IOKit"
 
 include($$ROOT_DIR/common.pri)
 
 SOURCES += \
   ../../testing/testingmain.cpp \
+  features_road_graph_test.cpp \
   routing_smoke.cpp \
   road_graph_builder.cpp \
   road_graph_builder_test.cpp \