Solving case when several features connect two joints.beta-495dobriy-eeh-index-routing10

3 files changed, 72 insertions, 19 deletions

diff --git a/routing/index_graph.cpp b/routing/index_graph.cpp
index d0e45261cc..f19beb672d 100644
--- a/routing/index_graph.cpp
+++ b/routing/index_graph.cpp
@@ -78,7 +78,7 @@ void IndexGraph::GetConnectionPaths(Joint::Id from, Joint::Id to,
   for (pair<RoadPoint, RoadPoint> const & c : connections)
   {
     vector<RoadPoint> roadPoints;
-    GetSingleFeaturePaths(c.first, c.second, roadPoints);
+    GetSingleFeaturePaths(c.first /* from */, c.second /* to */, roadPoints);
     connectionPaths.push_back(move(roadPoints));
   }
 }
@@ -127,14 +127,35 @@ void IndexGraph::GetShortestConnectionPath(Joint::Id from, Joint::Id to,
       shortestConnectionPath);
 }
 
+void IndexGraph::GetFeatureConnectionPath(Joint::Id from, Joint::Id to, uint32_t featureId,
+                                          vector<RoadPoint> & featureConnectionPath)
+{
+  vector<pair<RoadPoint, RoadPoint>> connections;
+  m_jointIndex.FindPointsWithCommonFeature(from, to, connections);
+  CHECK_LESS(0, connections.size(), ());
+
+  for (auto & c : connections)
+  {
+    CHECK_EQUAL(c.first.GetFeatureId(), c.second.GetFeatureId(), ());
+    if (c.first.GetFeatureId() == featureId)
+    {
+      GetSingleFeaturePaths(c.first /* from */, c.second /* to */, featureConnectionPath);
+      return;
+    }
+  }
+  MYTHROW(RootException,
+          ("Joint from:", from, "and joint to:", to, "are not connected feature:", featureId));
+}
+
 void IndexGraph::GetOutgoingGeomEdges(vector<JointEdge> const & outgoingEdges, Joint::Id center,
                                       vector<JointEdgeGeom> & outgoingGeomEdges)
 {
   for (JointEdge const & e : outgoingEdges)
   {
-    vector<RoadPoint> shortestConnectionPath;
-    GetShortestConnectionPath(center, e.GetTarget(), shortestConnectionPath);
-    outgoingGeomEdges.emplace_back(e.GetTarget(), shortestConnectionPath);
+    vector<vector<RoadPoint>> connectionPaths;
+    GetConnectionPaths(center, e.GetTarget(), connectionPaths);
+    for (auto const & path : connectionPaths)
+      outgoingGeomEdges.emplace_back(e.GetTarget(), path);
   }
 }
 
@@ -269,18 +290,16 @@ void IndexGraph::ApplyRestrictionNo(RoadPoint const & from, RoadPoint const & to
         || e.GetTarget() == centerId;
   }), outgoingEdges.end());
 
-  // Getting outgoing shortest edges geometry.
-  // Node. |centerId| could be connected with any outgoing joint with
-  // mote than one edge but only the shortest one should be takenn into
-  // account.
   my::SortUnique(outgoingEdges, my::LessBy(&JointEdge::GetTarget),
                  my::EqualsBy(&JointEdge::GetTarget));
+  // Node. |centerId| could be connected with any outgoing joint with more than one edge (feature).
+  // In GetOutgoingGeomEdges() below is taken into account the case.
   vector<JointEdgeGeom> outgoingShortestEdges;
   GetOutgoingGeomEdges(outgoingEdges, centerId, outgoingShortestEdges);
 
-  vector<RoadPoint> shortestIngoingPath;
-  GetShortestConnectionPath(fromFirstOneStepAside, centerId, shortestIngoingPath);
-  JointEdgeGeom ingoingShortestEdge(fromFirstOneStepAside, shortestIngoingPath);
+  vector<RoadPoint> ingoingPath;
+  GetFeatureConnectionPath(fromFirstOneStepAside, centerId, from.GetFeatureId(), ingoingPath);
+  JointEdgeGeom ingoingShortestEdge(fromFirstOneStepAside, ingoingPath);
 
   Joint::Id newJoint = Joint::kInvalidId;
   for (auto it = outgoingShortestEdges.cbegin();
@@ -373,13 +392,13 @@ void IndexGraph::ApplyRestrictionOnly(RoadPoint const & from, RoadPoint const &
   // *       *       *                     *       *       *
   // 4       5       7                     4       5       7
 
-  vector<RoadPoint> ingoingShortestPath;
-  GetShortestConnectionPath(fromFirstOneStepAside, centerId, ingoingShortestPath);
-  vector<RoadPoint> outgoingShortestPath;
-  GetShortestConnectionPath(centerId, toFirstOneStepAside, outgoingShortestPath);
-  CHECK_LESS(0, ingoingShortestPath.size(), ());
-  vector<RoadPoint> geometrySource(ingoingShortestPath.cbegin(), ingoingShortestPath.cend() - 1);
-  geometrySource.insert(geometrySource.end(), outgoingShortestPath.cbegin(), outgoingShortestPath.cend());
+  vector<RoadPoint> ingoingPath;
+  GetFeatureConnectionPath(fromFirstOneStepAside, centerId, from.GetFeatureId(), ingoingPath);
+  vector<RoadPoint> outgoingPath;
+  GetFeatureConnectionPath(centerId, toFirstOneStepAside, to.GetFeatureId(), outgoingPath);
+  CHECK_LESS(0, ingoingPath.size(), ());
+  vector<RoadPoint> geometrySource(ingoingPath.cbegin(), ingoingPath.cend() - 1);
+  geometrySource.insert(geometrySource.end(), outgoingPath.cbegin(), outgoingPath.cend());
 
   AddFakeFeature(fromFirstOneStepAside, toFirstOneStepAside, geometrySource);
   DisableEdge(fromFirstOneStepAside, centerId);
diff --git a/routing/index_graph.hpp b/routing/index_graph.hpp
index 413c8fbc54..33b0e74c30 100644
--- a/routing/index_graph.hpp
+++ b/routing/index_graph.hpp
@@ -39,7 +39,7 @@ class JointEdgeGeom final
 {
 public:
   JointEdgeGeom() = default;
-  JointEdgeGeom(Joint::Id target, vector<RoadPoint> & shortestPath)
+  JointEdgeGeom(Joint::Id target, vector<RoadPoint> const & shortestPath)
     : m_target(target), m_shortestPath(shortestPath)
   {
   }
@@ -159,6 +159,10 @@ public:
   void GetShortestConnectionPath(Joint::Id from, Joint::Id to,
                                  vector<RoadPoint> & shortestConnectionPath);
 
+  /// \brief Fills |featureConnectionPath| with a path from |from| to |to| of points of |featureId|.
+  void GetFeatureConnectionPath(Joint::Id from, Joint::Id to, uint32_t featureId,
+                                vector<RoadPoint> & featureConnectionPath);
+
   void GetOutgoingGeomEdges(vector<JointEdge> const & outgoingEdges, Joint::Id center,
                             vector<JointEdgeGeom> & outgoingGeomEdges);
 
diff --git a/routing/routing_tests/restriction_test.cpp b/routing/routing_tests/restriction_test.cpp
index 895481763e..6ebbad9a7e 100644
--- a/routing/routing_tests/restriction_test.cpp
+++ b/routing/routing_tests/restriction_test.cpp
@@ -619,4 +619,34 @@ UNIT_TEST(TwoWay_GetShortestConnectionPath)
   };
   TEST_EQUAL(shortestPath, expectedShortestPath, ());
 }
+
+UNIT_TEST(TwoWay_GetFeatureConnectionPath)
+{
+  unique_ptr<IndexGraph> graph = BuildTwoWayGraph();
+  vector<RoadPoint> featurePath;
+
+  // Full feature 0.
+  graph->GetFeatureConnectionPath(graph->GetJointIdForTesting({1 /* feature id */, 0 /* point id */}),
+    graph->GetJointIdForTesting({1, 3}), 0 /* feature id */, featurePath);
+  vector<RoadPoint> const expectedF0Path = {
+    {0, 0}, {0, 1}, {0, 2}
+  };
+  TEST_EQUAL(featurePath, expectedF0Path, ());
+
+  // Full feature 1.
+  graph->GetFeatureConnectionPath(graph->GetJointIdForTesting({1 /* feature id */, 0 /* point id */}),
+    graph->GetJointIdForTesting({1, 3}), 1 /* feature id */, featurePath);
+  vector<RoadPoint> const expectedF1Path = {
+    {1, 0}, {1, 1}, {1, 2}, {1, 3}
+  };
+  TEST_EQUAL(featurePath, expectedF1Path, ());
+
+  // Reversed full feature 0.
+  graph->GetFeatureConnectionPath(graph->GetJointIdForTesting({1 /* feature id */, 3 /* point id */}),
+    graph->GetJointIdForTesting({1, 0}), 0 /* feature id */, featurePath);
+  vector<RoadPoint> const expectedReversedF0Path = {
+    {0, 2}, {0, 1}, {0, 0}
+  };
+  TEST_EQUAL(featurePath, expectedReversedF0Path, ());
+}
 }  // namespace