[routing] fix, change algo

3 files changed, 55 insertions, 48 deletions

diff --git a/routing/index_graph_starter_joints.hpp b/routing/index_graph_starter_joints.hpp
index 991483810f..9c02ecc280 100644
--- a/routing/index_graph_starter_joints.hpp
+++ b/routing/index_graph_starter_joints.hpp
@@ -63,9 +63,9 @@ public:
   bool AreWavesConnectible(std::map<Vertex, Vertex> & forwardParents, Vertex const & commonVertex,
                           std::map<Vertex, Vertex> & backwardParents) override
   {
-    auto converter = [&](JointSegment const & vertex) {
-      if (vertex.IsRealSegment())
-        return vertex.GetFeatureId();
+    auto converter = [&](JointSegment const & vertex)
+    {
+      ASSERT(!vertex.IsRealSegment(), ());
 
       auto const it = m_fakeJointSegments.find(vertex);
       ASSERT(it != m_fakeJointSegments.cend(), ());
diff --git a/routing/restriction_loader.cpp b/routing/restriction_loader.cpp
index b5ef10efaf..6ef8f42242 100644
--- a/routing/restriction_loader.cpp
+++ b/routing/restriction_loader.cpp
@@ -80,31 +80,38 @@ RestrictionVec && RestrictionLoader::StealRestrictions()
   return std::move(m_restrictions);
 }
 
+bool IsRestrictionFromRoads(IndexGraph const & graph, std::vector<uint32_t> const & restriction)
+{
+  for (auto const & featureId : restriction)
+  {
+    if (!graph.IsRoad(featureId))
+      return false;
+  }
+
+  return true;
+}
+
 /// \brief We store |Only| restriction in mwm, like
 /// Only, featureId_1, ... , featureId_N
+///
 /// We convert such restriction to |No| following way:
-/// For each pair of neighbouring features (featureId_1 and featureId_2 for ex.)
-/// we forbid to go from featureId_1 to wherever except featureId_2.
-/// Then do this for featureId_2 and featureId_3 and so on.
+/// For each M, greater than 1, and M first features:
+/// featureId_1, ... , featureId_(M - 1), featureId_M, ...
+///
+/// We create restrictionNo from features:
+/// featureId_1, ... , featureId_(M - 1), featureId_K
+/// where featureId_K - has common joint with featureId_(M - 1) and featureId_M.
 void ConvertRestrictionsOnlyToNoAndSort(IndexGraph const & graph,
                                         RestrictionVec const & restrictionsOnly,
                                         RestrictionVec & restrictionsNo)
 {
-  for (auto const & restriction : restrictionsOnly)
+  for (std::vector<uint32_t> const & restriction : restrictionsOnly)
   {
-    if (std::any_of(restriction.begin(), restriction.end(),
-                    [&graph](auto const & item)
-                    {
-                      return !graph.IsRoad(item);
-                    }))
-    {
+    CHECK_GREATER_OR_EQUAL(restriction.size(), 2, ());
+
+    if (!IsRestrictionFromRoads(graph, restriction))
       continue;
-    }
 
-    CHECK_GREATER_OR_EQUAL(restriction.size(), 2, ());
-    // vector of simple no_restriction - from|to.
-    std::vector<std::pair<uint32_t, uint32_t>> toAdd;
-    bool error = false;
     for (size_t i = 1; i < restriction.size(); ++i)
     {
       auto const curFeatureId = restriction[i];
@@ -115,31 +122,22 @@ void ConvertRestrictionsOnlyToNoAndSort(IndexGraph const & graph,
           GetCommonEndJoint(graph.GetRoad(curFeatureId), graph.GetRoad(prevFeatureId));
 
       if (common == Joint::kInvalidId)
-      {
-        error = true;
         break;
-      }
 
-      // Adding restriction of type No for all features of joint |common| except for
-      // |prevFeatureId| -> |curFeatureId|.
+      std::vector<uint32_t> commonFeatures;
+      commonFeatures.resize(i + 1);
+      std::copy(restriction.begin(), restriction.begin() + i, commonFeatures.begin());
+
       graph.ForEachPoint(common,
                          [&](RoadPoint const & rp)
                          {
                            if (rp.GetFeatureId() != curFeatureId)
-                             toAdd.emplace_back(prevFeatureId, rp.GetFeatureId());
+                           {
+                             commonFeatures.back() = rp.GetFeatureId();
+                             restrictionsNo.emplace_back(commonFeatures);
+                           }
                          });
     }
-
-    if (error)
-      continue;
-
-    for (auto const & fromToPair : toAdd)
-    {
-      std::vector<uint32_t> newRestriction =
-          {fromToPair.first /* from */, fromToPair.second /* to */};
-
-      restrictionsNo.emplace_back(std::move(newRestriction));
-    }
   }
 
   base::SortUnique(restrictionsNo);
diff --git a/routing/single_vehicle_world_graph.cpp b/routing/single_vehicle_world_graph.cpp
index 575370948b..dd0ca951d9 100644
--- a/routing/single_vehicle_world_graph.cpp
+++ b/routing/single_vehicle_world_graph.cpp
@@ -251,6 +251,24 @@ void SingleVehicleWorldGraph::SetAStarParents(bool forward, map<JointSegment, Jo
 }
 
 template <typename VertexType>
+NumMwmId GetCommonMwmInChain(vector<VertexType> const & chain)
+{
+  NumMwmId mwmId = kFakeNumMwmId;
+  for (size_t i = 0; i < chain.size(); ++i)
+  {
+    if (chain[i].GetMwmId() == kFakeNumMwmId)
+      continue;
+
+    if (mwmId != kFakeNumMwmId && mwmId != chain[i].GetMwmId())
+      return kFakeNumMwmId;
+
+    mwmId = chain[i].GetMwmId();
+  }
+
+  return mwmId;
+}
+
+template <typename VertexType>
 bool 
 SingleVehicleWorldGraph::AreWavesConnectibleImpl(map<VertexType, VertexType> const & forwardParents,
                                                  VertexType const & commonVertex,
@@ -258,7 +276,6 @@ SingleVehicleWorldGraph::AreWavesConnectibleImpl(map<VertexType, VertexType> con
                                                  function<uint32_t(VertexType const &)> && fakeFeatureConverter)
 {
   vector<VertexType> chain;
-  NumMwmId mwmId = kFakeNumMwmId;
   auto const fillUntilNextFeatureId = [&](VertexType const & cur, map<VertexType, VertexType> const & parents)
   {
     auto startFeatureId = cur.GetFeatureId();
@@ -305,21 +322,13 @@ SingleVehicleWorldGraph::AreWavesConnectibleImpl(map<VertexType, VertexType> con
     }
   }
 
+  NumMwmId mwmId = GetCommonMwmInChain(chain);
+  if (mwmId == kFakeNumMwmId)
+    return true;
+
   map<VertexType, VertexType> parents;
   for (size_t i = 1; i < chain.size(); ++i)
-  {
     parents[chain[i]] = chain[i - 1];
-    if (chain[i].IsRealSegment())
-    {
-      if (mwmId != kFakeNumMwmId && mwmId != chain[i].GetMwmId())
-        return true;
-
-      mwmId = chain[i].GetMwmId();
-    }
-  }
-
-  if (mwmId == kFakeNumMwmId)
-    return true;
 
   auto & currentIndexGraph = GetIndexGraph(mwmId);
   for (size_t i = 1; i < chain.size(); ++i)