MLD direct shortest path plugin

src/engine/routing_algorithms/direct_shortest_path.cpp

@@ -2,6 +2,7 @@
 
 #include "engine/routing_algorithms/routing_base.hpp"
 #include "engine/routing_algorithms/routing_base_ch.hpp"
+#include "engine/routing_algorithms/routing_base_mld.hpp"
 
 namespace osrm
 {
@@ -10,14 +11,13 @@ namespace engine
 namespace routing_algorithms
 {
 
-namespace ch
-{
-
 template <typename AlgorithmT>
 InternalRouteResult
 extractRoute(const datafacade::ContiguousInternalMemoryDataFacade<AlgorithmT> &facade,
              const EdgeWeight weight,
-             const std::vector<NodeID> &packed_leg,
+             const NodeID source_node,
+             const NodeID target_node,
+             const std::vector<EdgeID> &edges,
              const PhantomNodes &nodes)
 {
     InternalRouteResult raw_route_data;
@@ -30,24 +30,25 @@ extractRoute(const datafacade::ContiguousInternalMemoryDataFacade<AlgorithmT> &f
         return raw_route_data;
     }
 
-    BOOST_ASSERT_MSG(!packed_leg.empty(), "packed path empty");
-
     raw_route_data.shortest_path_length = weight;
     raw_route_data.unpacked_path_segments.resize(1);
     raw_route_data.source_traversed_in_reverse.push_back(
-        (packed_leg.front() != nodes.source_phantom.forward_segment_id.id));
+        (source_node != nodes.source_phantom.forward_segment_id.id));
     raw_route_data.target_traversed_in_reverse.push_back(
-        (packed_leg.back() != nodes.target_phantom.forward_segment_id.id));
+        (target_node != nodes.target_phantom.forward_segment_id.id));
 
-    unpackPath(facade,
-               packed_leg.begin(),
-               packed_leg.end(),
-               nodes,
-               raw_route_data.unpacked_path_segments.front());
+    annotatePath(facade,
+                 source_node,
+                 target_node,
+                 edges,
+                 nodes,
+                 raw_route_data.unpacked_path_segments.front());
 
     return raw_route_data;
 }
 
+namespace ch
+{
 /// This is a striped down version of the general shortest path algorithm.
 /// The general algorithm always computes two queries for each leg. This is only
 /// necessary in case of vias, where the directions of the start node is constrainted
@@ -71,7 +72,7 @@ InternalRouteResult directShortestPathSearchImpl(
     forward_core_heap.Clear();
     reverse_core_heap.Clear();
 
-    int weight = INVALID_EDGE_WEIGHT;
+    EdgeWeight weight = INVALID_EDGE_WEIGHT;
     std::vector<NodeID> packed_leg;
     insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);
 
@@ -85,11 +86,27 @@ InternalRouteResult directShortestPathSearchImpl(
            DO_NOT_FORCE_LOOPS,
            DO_NOT_FORCE_LOOPS);
 
-    return extractRoute(facade, weight, packed_leg, phantom_nodes);
+    std::vector<EdgeID> unpacked_edges;
+    auto source_node = SPECIAL_NODEID, target_node = SPECIAL_NODEID;
+    if (!packed_leg.empty())
+    {
+        source_node = packed_leg.front();
+        target_node = packed_leg.back();
+        unpacked_edges.reserve(packed_leg.size());
+        unpackPath(
+            facade,
+            packed_leg.begin(),
+            packed_leg.end(),
+            [&facade, &unpacked_edges](std::pair<NodeID, NodeID> & /* edge */,
+                                       const auto &edge_id) { unpacked_edges.push_back(edge_id); });
+    }
+
+    return extractRoute(facade, weight, source_node, target_node, unpacked_edges, phantom_nodes);
 }
 
 } // namespace ch
 
+template <>
 InternalRouteResult directShortestPathSearch(
     SearchEngineData &engine_working_data,
     const datafacade::ContiguousInternalMemoryDataFacade<algorithm::CoreCH> &facade,
@@ -98,6 +115,7 @@ InternalRouteResult directShortestPathSearch(
     return ch::directShortestPathSearchImpl(engine_working_data, facade, phantom_nodes);
 }
 
+template <>
 InternalRouteResult directShortestPathSearch(
     SearchEngineData &engine_working_data,
     const datafacade::ContiguousInternalMemoryDataFacade<algorithm::CH> &facade,
@@ -106,6 +124,51 @@ InternalRouteResult directShortestPathSearch(
     return ch::directShortestPathSearchImpl(engine_working_data, facade, phantom_nodes);
 }
 
+template <>
+InternalRouteResult directShortestPathSearch(
+    SearchEngineData &engine_working_data,
+    const datafacade::ContiguousInternalMemoryDataFacade<algorithm::MLD> &facade,
+    const PhantomNodes &phantom_nodes)
+{
+    engine_working_data.InitializeOrClearMultiLayerDijkstraThreadLocalStorage(
+        facade.GetNumberOfNodes());
+    auto &forward_heap = *(engine_working_data.mld_forward_heap);
+    auto &reverse_heap = *(engine_working_data.mld_reverse_heap);
+    forward_heap.Clear();
+    reverse_heap.Clear();
+    insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);
+
+    const auto &partition = facade.GetMultiLevelPartition();
+    const auto &cells = facade.GetCellStorage();
+
+    auto get_highest_level = [&partition](const SegmentID &source, const SegmentID &target) {
+        if (source.enabled && target.enabled)
+            return partition.GetHighestDifferentLevel(source.id, target.id);
+        return INVALID_LEVEL_ID;
+    };
+
+    const auto &source_phantom = phantom_nodes.source_phantom;
+    const auto &target_phantom = phantom_nodes.target_phantom;
+    const auto highest_level =
+        std::min(std::min(get_highest_level(source_phantom.forward_segment_id,
+                                            target_phantom.forward_segment_id),
+                          get_highest_level(source_phantom.forward_segment_id,
+                                            target_phantom.reverse_segment_id)),
+                 std::min(get_highest_level(source_phantom.reverse_segment_id,
+                                            target_phantom.forward_segment_id),
+                          get_highest_level(source_phantom.reverse_segment_id,
+                                            target_phantom.reverse_segment_id)));
+    // TODO: when structured bindings will be allowed change to
+    // auto [weight, source_node, target_node, unpacked_edges] = ...
+    EdgeWeight weight;
+    NodeID source_node, target_node;
+    std::vector<EdgeID> unpacked_edges;
+    std::tie(weight, source_node, target_node, unpacked_edges) = mld::search(
+        facade, partition, cells, forward_heap, reverse_heap, {highest_level, INVALID_CELL_ID});
+
+    return extractRoute(facade, weight, source_node, target_node, unpacked_edges, phantom_nodes);
+}
+
 } // namespace routing_algorithms
 } // namespace engine
 } // namespace osrm

src/engine/search_engine_data.cpp

@@ -15,6 +15,9 @@ SearchEngineData::SearchEngineHeapPtr SearchEngineData::forward_heap_3;
 SearchEngineData::SearchEngineHeapPtr SearchEngineData::reverse_heap_3;
 SearchEngineData::ManyToManyHeapPtr SearchEngineData::many_to_many_heap;
 
+SearchEngineData::MultiLayerDijkstraHeapPtr SearchEngineData::mld_forward_heap;
+SearchEngineData::MultiLayerDijkstraHeapPtr SearchEngineData::mld_reverse_heap;
+
 void SearchEngineData::InitializeOrClearFirstThreadLocalStorage(const unsigned number_of_nodes)
 {
     if (forward_heap_1.get())
@@ -89,5 +92,27 @@ void SearchEngineData::InitializeOrClearManyToManyThreadLocalStorage(const unsig
         many_to_many_heap.reset(new ManyToManyQueryHeap(number_of_nodes));
     }
 }
+
+void SearchEngineData::InitializeOrClearMultiLayerDijkstraThreadLocalStorage(
+    const unsigned number_of_nodes)
+{
+    if (mld_forward_heap.get())
+    {
+        mld_forward_heap->Clear();
+    }
+    else
+    {
+        mld_forward_heap.reset(new MultiLayerDijkstraHeap(number_of_nodes));
+    }
+
+    if (mld_reverse_heap.get())
+    {
+        mld_reverse_heap->Clear();
+    }
+    else
+    {
+        mld_reverse_heap.reset(new MultiLayerDijkstraHeap(number_of_nodes));
+    }
+}
 }
 }