[routing] Add DijkstraWrapperJoints for calculation leaps in generator. Boost up to 35%.

1 files changed, 133 insertions, 20 deletions

diff --git a/generator/routing_index_generator.cpp b/generator/routing_index_generator.cpp
index 944a1954ee..91767cd8a8 100644
--- a/generator/routing_index_generator.cpp
+++ b/generator/routing_index_generator.cpp
@@ -8,9 +8,12 @@
 #include "routing/cross_mwm_connector.hpp"
 #include "routing/cross_mwm_connector_serialization.hpp"
 #include "routing/cross_mwm_ids.hpp"
+#include "routing/fake_feature_ids.hpp"
 #include "routing/index_graph.hpp"
 #include "routing/index_graph_loader.hpp"
 #include "routing/index_graph_serialization.hpp"
+#include "routing/index_graph_starter_joints.hpp"
+#include "routing/joint_segment.hpp"
 #include "routing/vehicle_mask.hpp"
 
 #include "routing_common/bicycle_model.hpp"
@@ -40,6 +43,7 @@
 #include <algorithm>
 #include <cstdint>
 #include <functional>
+#include <limits>
 #include <map>
 #include <memory>
 #include <unordered_map>
@@ -151,26 +155,69 @@ private:
   unordered_map<uint32_t, VehicleMask> m_masks;
 };
 
-class DijkstraWrapper final
+class IndexGraphWrapper final
+{
+public:
+  IndexGraphWrapper(IndexGraph & graph, Segment const & start)
+    : m_graph(graph), m_start(start) {}
+
+  // Just for compatibility with IndexGraphStarterJoints
+  // @{
+  Segment GetStartSegment() const { return m_start; }
+  Segment GetFinishSegment() const { return {}; }
+  bool ConvertToReal(Segment const & /* segment */) const { return false; }
+  // @}
+
+  m2::PointD const & GetPoint(Segment const & s, bool forward)
+  {
+    return m_graph.GetPoint(s, forward);
+  }
+
+  void GetEdgesList(Segment const & from, bool isOutgoing, std::vector<SegmentEdge> & edges)
+  {
+    m_graph.GetEdgeList(from, isOutgoing, edges);
+  }
+
+  void GetEdgeList(Segment const & segment, bool isOutgoing,
+                   std::vector<JointEdge> & edges, std::vector<RouteWeight> & parentWeights) const
+  {
+    return m_graph.GetEdgeList(segment, isOutgoing, edges, parentWeights);
+  }
+
+  bool IsJoint(Segment const & segment, bool fromStart) const
+  {
+    if (m_graph.IsJoint(segment.GetRoadPoint(fromStart)))
+      return true;
+
+    uint32_t const pointId = segment.GetPointId(fromStart);
+    uint32_t const pointsNumber = m_graph.GetGeometry().GetRoad(segment.GetFeatureId()).GetPointsCount();
+    return pointId == 0 || pointId + 1 == pointsNumber;
+  }
+
+private:
+  IndexGraph & m_graph;
+  Segment m_start;
+};
+
+class DijkstraWrapperJoints final
 {
 public:
   // AStarAlgorithm types aliases:
-  using Vertex = Segment;
-  using Edge = SegmentEdge;
+  using Vertex = JointSegment;
+  using Edge = JointEdge;
   using Weight = RouteWeight;
 
-  explicit DijkstraWrapper(IndexGraph & graph) : m_graph(graph) {}
+  explicit DijkstraWrapperJoints(IndexGraphWrapper & graph, Segment const & start)
+    : m_graph(graph, start) {}
 
   void GetOutgoingEdgesList(Vertex const & vertex, vector<Edge> & edges)
   {
-    edges.clear();
-    m_graph.GetEdgeList(vertex, true /* isOutgoing */, edges);
+    m_graph.GetOutgoingEdgesList(vertex, edges);
   }
 
   void GetIngoingEdgesList(Vertex const & vertex, vector<Edge> & edges)
   {
-    edges.clear();
-    m_graph.GetEdgeList(vertex, false /* isOutgoing */, edges);
+    m_graph.GetIngoingEdgesList(vertex, edges);
   }
 
   Weight HeuristicCostEstimate(Vertex const & /* from */, Vertex const & /* to */)
@@ -178,8 +225,15 @@ public:
     return GetAStarWeightZero<Weight>();
   }
 
+  m2::PointD const & GetPoint(Vertex const & vertex, bool forward)
+  {
+    return m_graph.GetPoint(vertex, forward);
+  }
+
+  IndexGraphStarterJoints<IndexGraphWrapper> & GetGraph() { return m_graph; }
+
 private:
-  IndexGraph & m_graph;
+  IndexGraphStarterJoints<IndexGraphWrapper> m_graph;
 };
 
 // Calculate distance from the starting border point to the transition along the border.
@@ -404,28 +458,87 @@ void FillWeights(string const & path, string const & mwmFile, string const & cou
   size_t notFoundCount = 0;
   for (size_t i = 0; i < numEnters; ++i)
   {
-    if (!disableCrossMwmProgress && (i % 10 == 0) && (i != 0))
+    if (i % 10 == 0)
       LOG(LINFO, ("Building leaps:", i, "/", numEnters, "waves passed"));
 
     Segment const & enter = connector.GetEnter(i);
 
-    AStarAlgorithm<DijkstraWrapper> astar;
-    DijkstraWrapper wrapper(graph);
-    AStarAlgorithm<DijkstraWrapper>::Context context;
-    astar.PropagateWave(wrapper, enter,
-                        [](Segment const & /* vertex */) { return true; } /* visitVertex */,
+
+    AStarAlgorithm<DijkstraWrapperJoints> astar;
+    IndexGraphWrapper indexGraphWrapper(graph, enter);
+    DijkstraWrapperJoints wrapper(indexGraphWrapper, enter);
+    AStarAlgorithm<DijkstraWrapperJoints>::Context context;
+    std::unordered_map<uint32_t, std::vector<JointSegment>> visitedVertexes;
+    astar.PropagateWave(wrapper, wrapper.GetGraph().GetStartJoint(),
+                        [&](JointSegment const & vertex)
+                        {
+                          if (vertex.IsFake())
+                          {
+                            Segment start = wrapper.GetGraph().GetSegmentOfFakeJoint(vertex, true /* start */);
+                            Segment end = wrapper.GetGraph().GetSegmentOfFakeJoint(vertex, false /* start */);
+                            if (start.IsForward() != end.IsForward())
+                              return true;
+
+                            visitedVertexes[end.GetFeatureId()].emplace_back(start, end);
+                          }
+                          else
+                          {
+                            visitedVertexes[vertex.GetFeatureId()].emplace_back(vertex);
+                          }
+
+                          return true;
+                        } /* visitVertex */,
                         context);
 
     for (Segment const & exit : connector.GetExits())
     {
-      if (context.HasDistance(exit))
+      auto const it = visitedVertexes.find(exit.GetFeatureId());
+      if (it == visitedVertexes.cend())
       {
-        weights[enter][exit] = context.GetDistance(exit);
-        ++foundCount;
+        ++notFoundCount;
+        continue;
       }
-      else
+
+      uint32_t const id = exit.GetSegmentIdx();
+      bool const forward = exit.IsForward();
+      for (auto const & jointSegment : it->second)
       {
-        ++notFoundCount;
+        if (jointSegment.IsForward() != forward)
+          continue;
+
+        if ((jointSegment.GetStartSegmentId() <= id && id <= jointSegment.GetEndSegmentId()) ||
+            (jointSegment.GetEndSegmentId() <= id && id <= jointSegment.GetStartSegmentId()))
+        {
+          RouteWeight weight;
+          Segment parentSegment;
+          if (context.HasParent(jointSegment))
+          {
+            JointSegment const & parent = context.GetParent(jointSegment);
+            parentSegment = parent.IsFake() ? wrapper.GetGraph().GetSegmentOfFakeJoint(parent, false /* start */)
+                                            : parent.GetSegment(false /* start */);
+
+            weight = context.GetDistance(parent);
+          }
+          else
+          {
+            parentSegment = enter;
+          }
+
+          Segment const & firstChild = jointSegment.GetSegment(true /* start */);
+          uint32_t const lastPoint = exit.GetPointId(true /* front */);
+
+          auto optionalEdge =  graph.GetJointEdgeByLastPoint(parentSegment, firstChild,
+                                                             true /* isOutgoing */, lastPoint);
+
+          if (!optionalEdge)
+            continue;
+
+          weight += (*optionalEdge).GetWeight();
+          weights[enter][exit] = weight;
+
+          ++foundCount;
+          break;
+        }
       }
     }
   }