Add support for annotations=distances in MLD

This commit brings feature parity with CH for the `table` pluging.

include/engine/routing_algorithms/many_to_many.hpp

@@ -15,24 +15,25 @@ namespace engine
 {
 namespace routing_algorithms
 {
-
 namespace
 {
 struct NodeBucket
 {
     NodeID middle_node;
     NodeID parent_node;
+    bool from_clique_arc;
     unsigned column_index; // a column in the weight/duration matrix
     EdgeWeight weight;
     EdgeDuration duration;
 
     NodeBucket(NodeID middle_node,
                NodeID parent_node,
+               bool from_clique_arc,
                unsigned column_index,
                EdgeWeight weight,
                EdgeDuration duration)
-        : middle_node(middle_node), parent_node(parent_node), column_index(column_index),
-          weight(weight), duration(duration)
+        : middle_node(middle_node), parent_node(parent_node), from_clique_arc(from_clique_arc),
+          column_index(column_index), weight(weight), duration(duration)
     {
     }
 

include/engine/routing_algorithms/routing_base_mld.hpp

@@ -54,7 +54,7 @@ inline bool checkParentCellRestriction(CellID, const PhantomNodes &) { return tr
 
 // Restricted search (Args is LevelID, CellID):
 //   * use the fixed level for queries
-//   * check if the node cell is the same as the specified parent onr
+//   * check if the node cell is the same as the specified parent
 template <typename MultiLevelPartition>
 inline LevelID getNodeQueryLevel(const MultiLevelPartition &, NodeID, LevelID level, CellID)
 {
@@ -65,6 +65,61 @@ inline bool checkParentCellRestriction(CellID cell, LevelID, CellID parent)
 {
     return cell == parent;
 }
+
+// Unrestricted search with a single phantom node (Args is const PhantomNode &):
+//   * use partition.GetQueryLevel to find the node query level
+//   * allow to traverse all cells
+template <typename MultiLevelPartition>
+inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
+                                 const NodeID node,
+                                 const PhantomNode &phantom_node)
+{
+    auto highest_diffrent_level = [&partition, node](const SegmentID &phantom_node) {
+        if (phantom_node.enabled)
+            return partition.GetHighestDifferentLevel(phantom_node.id, node);
+        return INVALID_LEVEL_ID;
+    };
+
+    const auto node_level = std::min(highest_diffrent_level(phantom_node.forward_segment_id),
+                                     highest_diffrent_level(phantom_node.reverse_segment_id));
+
+    return node_level;
+}
+
+// Unrestricted search with a single phantom node and a vector of phantom nodes:
+//   * use partition.GetQueryLevel to find the node query level
+//   * allow to traverse all cells
+template <typename MultiLevelPartition>
+inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
+                                 NodeID node,
+                                 const std::vector<PhantomNode> &phantom_nodes,
+                                 const std::size_t phantom_index,
+                                 const std::vector<std::size_t> &phantom_indices)
+{
+    auto min_level = [&partition, node](const PhantomNode &phantom_node) {
+
+        const auto &forward_segment = phantom_node.forward_segment_id;
+        const auto forward_level =
+            forward_segment.enabled ? partition.GetHighestDifferentLevel(node, forward_segment.id)
+                                    : INVALID_LEVEL_ID;
+
+        const auto &reverse_segment = phantom_node.reverse_segment_id;
+        const auto reverse_level =
+            reverse_segment.enabled ? partition.GetHighestDifferentLevel(node, reverse_segment.id)
+                                    : INVALID_LEVEL_ID;
+
+        return std::min(forward_level, reverse_level);
+    };
+
+    // Get minimum level over all phantoms of the highest different level with respect to node
+    // This is equivalent to min_{∀ source, target} partition.GetQueryLevel(source, node, target)
+    auto result = min_level(phantom_nodes[phantom_index]);
+    for (const auto &index : phantom_indices)
+    {
+        result = std::min(result, min_level(phantom_nodes[index]));
+    }
+    return result;
+}
 }
 
 // Heaps only record for each node its predecessor ("parent") on the shortest path.
@@ -74,6 +129,46 @@ inline bool checkParentCellRestriction(CellID cell, LevelID, CellID parent)
 using PackedEdge = std::tuple</*from*/ NodeID, /*to*/ NodeID, /*from_clique_arc*/ bool>;
 using PackedPath = std::vector<PackedEdge>;
 
+template <bool DIRECTION, typename OutIter>
+inline void retrievePackedPathFromSingleManyToManyHeap(
+    const SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap, const NodeID middle, OutIter out)
+{
+
+    NodeID current = middle;
+    NodeID parent = heap.GetData(current).parent;
+
+    while (current != parent)
+    {
+        const auto &data = heap.GetData(current);
+
+        if (DIRECTION == FORWARD_DIRECTION)
+        {
+            *out = std::make_tuple(parent, current, data.from_clique_arc);
+            ++out;
+        }
+        else if (DIRECTION == REVERSE_DIRECTION)
+        {
+            *out = std::make_tuple(current, parent, data.from_clique_arc);
+            ++out;
+        }
+
+        current = parent;
+        parent = heap.GetData(parent).parent;
+    }
+}
+
+template <bool DIRECTION>
+inline PackedPath retrievePackedPathFromSingleManyToManyHeap(
+    const SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap, const NodeID middle)
+{
+
+    PackedPath packed_path;
+    retrievePackedPathFromSingleManyToManyHeap<DIRECTION>(
+        heap, middle, std::back_inserter(packed_path));
+
+    return packed_path;
+}
+
 template <bool DIRECTION, typename OutIter>
 inline void retrievePackedPathFromSingleHeap(const SearchEngineData<Algorithm>::QueryHeap &heap,
                                              const NodeID middle,
@@ -351,6 +446,21 @@ UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
     // Get packed path as edges {from node ID, to node ID, from_clique_arc}
     auto packed_path = retrievePackedPathFromHeap(forward_heap, reverse_heap, middle);
 
+    // if (!packed_path.empty())
+    // {
+    //     std::cout << "packed_path: ";
+    //     for (auto edge : packed_path)
+    //     {
+    //         std::cout << std::get<0>(edge) << ",";
+    //     }
+    //     std::cout << std::get<1>(packed_path.back());
+    //     std::cout << std::endl;
+    // }
+    // else
+    // {
+    //     std::cout << "no packed_path!" << std::endl;
+    // }
+
     // Beware the edge case when start, middle, end are all the same.
     // In this case we return a single node, no edges. We also don't unpack.
     const NodeID source_node = !packed_path.empty() ? std::get<0>(packed_path.front()) : middle;
@@ -410,7 +520,96 @@ UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
             unpacked_edges.insert(unpacked_edges.end(), subpath_edges.begin(), subpath_edges.end());
         }
     }
+    // std::cout << "unpacked_nodes: ";
+    // for (auto node : unpacked_nodes)
+    // {
+    //     std::cout << node << ", ";
+    // }
+    // std::cout << std::endl;
+    return std::make_tuple(weight, std::move(unpacked_nodes), std::move(unpacked_edges));
+}
 
+// With (s, middle, t) we trace back the paths middle -> s and middle -> t.
+// This gives us a packed path (node ids) from the base graph around s and t,
+// and overlay node ids otherwise. We then have to unpack the overlay clique
+// edges by recursively descending unpacking the path down to the base graph.
+
+using UnpackedNodes = std::vector<NodeID>;
+using UnpackedEdges = std::vector<EdgeID>;
+using UnpackedPath = std::tuple<EdgeWeight, UnpackedNodes, UnpackedEdges>;
+
+template <typename Algorithm, typename... Args>
+UnpackedPath
+unpackPathAndCalculateDistance(SearchEngineData<Algorithm> &engine_working_data,
+                               const DataFacade<Algorithm> &facade,
+                               typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
+                               typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
+                               const bool force_loop_forward,
+                               const bool force_loop_reverse,
+                               EdgeWeight weight_upper_bound,
+                               PackedPath packed_path,
+                               NodeID middle,
+                               Args... args)
+{
+    EdgeWeight weight = weight_upper_bound;
+    const auto &partition = facade.GetMultiLevelPartition();
+    const NodeID source_node = !packed_path.empty() ? std::get<0>(packed_path.front()) : middle;
+
+    // Unpack path
+    std::vector<NodeID> unpacked_nodes;
+    std::vector<EdgeID> unpacked_edges;
+    unpacked_nodes.reserve(packed_path.size());
+    unpacked_edges.reserve(packed_path.size());
+
+    unpacked_nodes.push_back(source_node);
+
+    for (auto const &packed_edge : packed_path)
+    {
+        NodeID source, target;
+        bool overlay_edge;
+        std::tie(source, target, overlay_edge) = packed_edge;
+        if (!overlay_edge)
+        { // a base graph edge
+            unpacked_nodes.push_back(target);
+            unpacked_edges.push_back(facade.FindEdge(source, target));
+        }
+        else
+        { // an overlay graph edge
+            LevelID level = getNodeQueryLevel(partition, source, args...);
+            CellID parent_cell_id = partition.GetCell(level, source);
+            BOOST_ASSERT(parent_cell_id == partition.GetCell(level, target));
+
+            LevelID sublevel = level - 1;
+
+            // Here heaps can be reused, let's go deeper!
+            forward_heap.Clear();
+            reverse_heap.Clear();
+            forward_heap.Insert(source, 0, {source});
+            reverse_heap.Insert(target, 0, {target});
+
+            // TODO: when structured bindings will be allowed change to
+            // auto [subpath_weight, subpath_source, subpath_target, subpath] = ...
+            EdgeWeight subpath_weight;
+            std::vector<NodeID> subpath_nodes;
+            std::vector<EdgeID> subpath_edges;
+            std::tie(subpath_weight, subpath_nodes, subpath_edges) = search(engine_working_data,
+                                                                            facade,
+                                                                            forward_heap,
+                                                                            reverse_heap,
+                                                                            force_loop_forward,
+                                                                            force_loop_reverse,
+                                                                            weight_upper_bound,
+                                                                            sublevel,
+                                                                            parent_cell_id);
+            BOOST_ASSERT(!subpath_edges.empty());
+            BOOST_ASSERT(subpath_nodes.size() > 1);
+            BOOST_ASSERT(subpath_nodes.front() == source);
+            BOOST_ASSERT(subpath_nodes.back() == target);
+            unpacked_nodes.insert(
+                unpacked_nodes.end(), std::next(subpath_nodes.begin()), subpath_nodes.end());
+            unpacked_edges.insert(unpacked_edges.end(), subpath_edges.begin(), subpath_edges.end());
+        }
+    }
     return std::make_tuple(weight, std::move(unpacked_nodes), std::move(unpacked_edges));
 }
 

include/util/query_heap.hpp

@@ -226,12 +226,14 @@ class QueryHeap
     Data &GetData(NodeID node)
     {
         const auto index = node_index.peek_index(node);
+        BOOST_ASSERT((int)index >= 0 && (int)index < (int)inserted_nodes.size());
         return inserted_nodes[index].data;
     }
 
     Data const &GetData(NodeID node) const
     {
         const auto index = node_index.peek_index(node);
+        BOOST_ASSERT((int)index >= 0 && (int)index < (int)inserted_nodes.size());
         return inserted_nodes[index].data;
     }