From cc423f1de51b24f2abe310a321401a08f9004985 Mon Sep 17 00:00:00 2001 From: Siarhei Fedartsou Date: Mon, 20 May 2024 11:48:48 +0200 Subject: [PATCH] Optimise getNetworkDistance in MLD even more --- CMakeLists.txt | 2 +- .../routing_algorithms/routing_base_mld.hpp | 264 +++++++++++++++--- include/engine/search_engine_data.hpp | 28 ++ .../routing_algorithms/map_matching.cpp | 9 +- src/engine/search_engine_data.cpp | 50 ++++ 5 files changed, 308 insertions(+), 45 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 23bd0bcad..ce55021ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -359,7 +359,7 @@ if(ENABLE_CONAN) KEEP_RPATHS NO_OUTPUT_DIRS OPTIONS boost:filesystem_version=3 # https://stackoverflow.com/questions/73392648/error-with-boost-filesystem-version-in-cmake - onetbb:shared=${TBB_SHARED} + # onetbb:shared=${TBB_SHARED} boost:without_stacktrace=True # Apple Silicon cross-compilation fails without it BUILD missing ) diff --git a/include/engine/routing_algorithms/routing_base_mld.hpp b/include/engine/routing_algorithms/routing_base_mld.hpp index b85ce5496..d64bc0562 100644 --- a/include/engine/routing_algorithms/routing_base_mld.hpp +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -384,9 +384,134 @@ void relaxOutgoingEdges(const DataFacade &facade, } template +void relaxOutgoingEdges( + const DataFacade &facade, + typename SearchEngineData::MapMatchingQueryHeap &forward_heap, + const typename SearchEngineData::MapMatchingQueryHeap::HeapNode &heapNode, + const Args &...args) +{ + const auto &partition = facade.GetMultiLevelPartition(); + const auto &cells = facade.GetCellStorage(); + const auto &metric = facade.GetCellMetric(); + + const auto level = getNodeQueryLevel(partition, heapNode.node, args...); + + if (level >= 1 && !heapNode.data.from_clique_arc) + { + if constexpr (DIRECTION == FORWARD_DIRECTION) + { + // Shortcuts in forward direction + const auto &cell = + cells.GetCell(metric, level, partition.GetCell(level, heapNode.node)); + auto destination = cell.GetDestinationNodes().begin(); + auto distance = cell.GetOutDistance(heapNode.node).begin(); + for (auto shortcut_weight : cell.GetOutWeight(heapNode.node)) + { + BOOST_ASSERT(destination != cell.GetDestinationNodes().end()); + const NodeID to = *destination; + + if (shortcut_weight != INVALID_EDGE_WEIGHT && heapNode.node != to) + { + const EdgeWeight to_weight = heapNode.weight + shortcut_weight; + const EdgeDistance to_distance = heapNode.data.distance + *distance; + BOOST_ASSERT(to_weight >= heapNode.weight); + const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to); + if (!toHeapNode) + { + forward_heap.Insert(to, to_weight, {heapNode.node, true, to_distance}); + } + else if (to_weight < toHeapNode->weight) + { + toHeapNode->data = {heapNode.node, true, to_distance}; + toHeapNode->weight = to_weight; + forward_heap.DecreaseKey(*toHeapNode); + } + } + ++destination; + ++distance; + } + } + else + { + // Shortcuts in backward direction + const auto &cell = + cells.GetCell(metric, level, partition.GetCell(level, heapNode.node)); + auto source = cell.GetSourceNodes().begin(); + auto distance = cell.GetInDistance(heapNode.node).begin(); + for (auto shortcut_weight : cell.GetInWeight(heapNode.node)) + { + BOOST_ASSERT(source != cell.GetSourceNodes().end()); + const NodeID to = *source; + + if (shortcut_weight != INVALID_EDGE_WEIGHT && heapNode.node != to) + { + const EdgeWeight to_weight = heapNode.weight + shortcut_weight; + const EdgeDistance to_distance = heapNode.data.distance + *distance; + BOOST_ASSERT(to_weight >= heapNode.weight); + const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to); + if (!toHeapNode) + { + forward_heap.Insert(to, to_weight, {heapNode.node, true, to_distance}); + } + else if (to_weight < toHeapNode->weight) + { + toHeapNode->data = {heapNode.node, true, to_distance}; + toHeapNode->weight = to_weight; + forward_heap.DecreaseKey(*toHeapNode); + } + } + ++source; + ++distance; + } + } + } + + // Boundary edges + for (const auto edge : facade.GetBorderEdgeRange(level, heapNode.node)) + { + const auto &edge_data = facade.GetEdgeData(edge); + + if ((DIRECTION == FORWARD_DIRECTION) ? facade.IsForwardEdge(edge) + : facade.IsBackwardEdge(edge)) + { + const NodeID to = facade.GetTarget(edge); + + if (!facade.ExcludeNode(to) && + checkParentCellRestriction(partition.GetCell(level + 1, to), args...)) + { + const auto node_weight = + facade.GetNodeWeight(DIRECTION == FORWARD_DIRECTION ? heapNode.node : to); + const auto node_distance = + facade.GetNodeDistance(DIRECTION == FORWARD_DIRECTION ? heapNode.node : to); + + const auto turn_penalty = facade.GetWeightPenaltyForEdgeID(edge_data.turn_id); + + // TODO: BOOST_ASSERT(edge_data.weight == node_weight + turn_penalty); + + const EdgeWeight to_weight = + heapNode.weight + node_weight + alias_cast(turn_penalty); + const EdgeDistance to_distance = heapNode.data.distance + node_distance; + + const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to); + if (!toHeapNode) + { + forward_heap.Insert(to, to_weight, {heapNode.node, false, to_distance}); + } + else if (to_weight < toHeapNode->weight) + { + toHeapNode->data = {heapNode.node, false, to_distance}; + toHeapNode->weight = to_weight; + forward_heap.DecreaseKey(*toHeapNode); + } + } + } + } +} + +template void routingStep(const DataFacade &facade, - typename SearchEngineData::QueryHeap &forward_heap, - typename SearchEngineData::QueryHeap &reverse_heap, + Heap &forward_heap, + Heap &reverse_heap, NodeID &middle_node, EdgeWeight &path_upper_bound, const std::vector &force_step_nodes, @@ -478,6 +603,9 @@ UnpackedPath search(SearchEngineData &engine_working_data, return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector(), std::vector()); } + // std::cerr << "Distance = " << forward_heap.GetData(middle).distance << " " << + // reverse_heap.GetData(middle).distance << std::endl; + // Get packed path as edges {from node ID, to node ID, from_clique_arc} auto packed_path = retrievePackedPathFromHeap(forward_heap, reverse_heap, middle); @@ -536,6 +664,61 @@ UnpackedPath search(SearchEngineData &engine_working_data, return std::make_tuple(weight, std::move(unpacked_nodes), std::move(unpacked_edges)); } +template +EdgeDistance +searchDistance(SearchEngineData &, + const DataFacade &facade, + typename SearchEngineData::MapMatchingQueryHeap &forward_heap, + typename SearchEngineData::MapMatchingQueryHeap &reverse_heap, + const std::vector &force_step_nodes, + EdgeWeight weight_upper_bound, + const Args &...args) +{ + if (forward_heap.Empty() || reverse_heap.Empty()) + { + return INVALID_EDGE_DISTANCE; + } + + // const auto &partition = facade.GetMultiLevelPartition(); + + BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT); + BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT); + + // run two-Target Dijkstra routing step. + NodeID middle = SPECIAL_NODEID; + EdgeWeight weight = weight_upper_bound; + EdgeWeight forward_heap_min = forward_heap.MinKey(); + EdgeWeight reverse_heap_min = reverse_heap.MinKey(); + while (forward_heap.Size() + reverse_heap.Size() > 0 && + forward_heap_min + reverse_heap_min < weight) + { + if (!forward_heap.Empty()) + { + routingStep( + facade, forward_heap, reverse_heap, middle, weight, force_step_nodes, args...); + if (!forward_heap.Empty()) + forward_heap_min = forward_heap.MinKey(); + } + if (!reverse_heap.Empty()) + { + routingStep( + facade, reverse_heap, forward_heap, middle, weight, force_step_nodes, args...); + if (!reverse_heap.Empty()) + reverse_heap_min = reverse_heap.MinKey(); + } + }; + + // No path found for both target nodes? + if (weight >= weight_upper_bound || SPECIAL_NODEID == middle) + { + return INVALID_EDGE_DISTANCE; + } + + auto distance = forward_heap.GetData(middle).distance + reverse_heap.GetData(middle).distance; + + return distance; +} + // Alias to be compatible with the CH-based search template inline void search(SearchEngineData &engine_working_data, @@ -593,8 +776,8 @@ void unpackPath(const FacadeT &facade, template double getNetworkDistance(SearchEngineData &engine_working_data, const DataFacade &facade, - typename SearchEngineData::QueryHeap &forward_heap, - typename SearchEngineData::QueryHeap &reverse_heap, + typename SearchEngineData::MapMatchingQueryHeap &forward_heap, + typename SearchEngineData::MapMatchingQueryHeap &reverse_heap, const PhantomNode &source_phantom, const PhantomNode &target_phantom, EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT) @@ -602,48 +785,49 @@ double getNetworkDistance(SearchEngineData &engine_working_data, forward_heap.Clear(); reverse_heap.Clear(); - const PhantomEndpoints endpoints{source_phantom, target_phantom}; - insertNodesInHeaps(forward_heap, reverse_heap, endpoints); + if (source_phantom.IsValidForwardSource()) + { + forward_heap.Insert(source_phantom.forward_segment_id.id, + EdgeWeight{0} - source_phantom.GetForwardWeightPlusOffset(), + {source_phantom.forward_segment_id.id, + false, + EdgeDistance{0} - source_phantom.GetForwardDistance()}); + } - auto [weight, unpacked_nodes, unpacked_edges] = search( + if (source_phantom.IsValidReverseSource()) + { + forward_heap.Insert(source_phantom.reverse_segment_id.id, + EdgeWeight{0} - source_phantom.GetReverseWeightPlusOffset(), + {source_phantom.reverse_segment_id.id, + false, + EdgeDistance{0} - source_phantom.GetReverseDistance()}); + } + + if (target_phantom.IsValidForwardTarget()) + { + reverse_heap.Insert( + target_phantom.forward_segment_id.id, + target_phantom.GetForwardWeightPlusOffset(), + {target_phantom.forward_segment_id.id, false, target_phantom.GetForwardDistance()}); + } + + if (target_phantom.IsValidReverseTarget()) + { + reverse_heap.Insert( + target_phantom.reverse_segment_id.id, + target_phantom.GetReverseWeightPlusOffset(), + {target_phantom.reverse_segment_id.id, false, target_phantom.GetReverseDistance()}); + } + + const PhantomEndpoints endpoints{source_phantom, target_phantom}; + + auto distance = searchDistance( engine_working_data, facade, forward_heap, reverse_heap, {}, weight_upper_bound, endpoints); - if (weight == INVALID_EDGE_WEIGHT) + if (distance == INVALID_EDGE_DISTANCE) { return std::numeric_limits::max(); } - - BOOST_ASSERT(unpacked_nodes.size() >= 1); - - EdgeDistance distance = {0.0}; - - if (source_phantom.forward_segment_id.id == unpacked_nodes.front()) - { - BOOST_ASSERT(source_phantom.forward_segment_id.enabled); - distance = EdgeDistance{0} - source_phantom.GetForwardDistance(); - } - else if (source_phantom.reverse_segment_id.id == unpacked_nodes.front()) - { - BOOST_ASSERT(source_phantom.reverse_segment_id.enabled); - distance = EdgeDistance{0} - source_phantom.GetReverseDistance(); - } - - for (size_t index = 0; index < unpacked_nodes.size() - 1; ++index) - { - distance += facade.GetNodeDistance(unpacked_nodes[index]); - } - - if (target_phantom.forward_segment_id.id == unpacked_nodes.back()) - { - BOOST_ASSERT(target_phantom.forward_segment_id.enabled); - distance += target_phantom.GetForwardDistance(); - } - else if (target_phantom.reverse_segment_id.id == unpacked_nodes.back()) - { - BOOST_ASSERT(target_phantom.reverse_segment_id.enabled); - distance += target_phantom.GetReverseDistance(); - } - return from_alias(distance); } diff --git a/include/engine/search_engine_data.hpp b/include/engine/search_engine_data.hpp index 2f32b327b..f391f11ce 100644 --- a/include/engine/search_engine_data.hpp +++ b/include/engine/search_engine_data.hpp @@ -56,6 +56,10 @@ template <> struct SearchEngineData static SearchEngineHeapPtr forward_heap_3; static SearchEngineHeapPtr reverse_heap_3; static ManyToManyHeapPtr many_to_many_heap; + static SearchEngineHeapPtr map_matching_forward_heap_1; + static SearchEngineHeapPtr map_matching_reverse_heap_1; + + void InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes); void InitializeOrClearFirstThreadLocalStorage(unsigned number_of_nodes); @@ -74,6 +78,19 @@ struct MultiLayerDijkstraHeapData MultiLayerDijkstraHeapData(NodeID p, bool from) : parent(p), from_clique_arc(from) {} }; +struct MapMatchingMultiLayerDijkstraHeapData +{ + NodeID parent; + bool from_clique_arc; + EdgeDistance distance = {0}; + MapMatchingMultiLayerDijkstraHeapData(NodeID p) : parent(p), from_clique_arc(false) {} + MapMatchingMultiLayerDijkstraHeapData(NodeID p, bool from) : parent(p), from_clique_arc(from) {} + MapMatchingMultiLayerDijkstraHeapData(NodeID p, bool from, EdgeDistance d) + : parent(p), from_clique_arc(from), distance(d) + { + } +}; + struct ManyToManyMultiLayerDijkstraHeapData : MultiLayerDijkstraHeapData { EdgeDuration duration; @@ -104,16 +121,27 @@ template <> struct SearchEngineData EdgeWeight, ManyToManyMultiLayerDijkstraHeapData, util::TwoLevelStorage>; + using MapMatchingQueryHeap = util::QueryHeap>; using SearchEngineHeapPtr = boost::thread_specific_ptr; using ManyToManyHeapPtr = boost::thread_specific_ptr; + using MapMatchingHeapPtr = boost::thread_specific_ptr; static SearchEngineHeapPtr forward_heap_1; static SearchEngineHeapPtr reverse_heap_1; + static MapMatchingHeapPtr map_matching_forward_heap_1; + static MapMatchingHeapPtr map_matching_reverse_heap_1; + static ManyToManyHeapPtr many_to_many_heap; void InitializeOrClearFirstThreadLocalStorage(unsigned number_of_nodes, unsigned number_of_boundary_nodes); + void InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes, + unsigned number_of_boundary_nodes); void InitializeOrClearManyToManyThreadLocalStorage(unsigned number_of_nodes, unsigned number_of_boundary_nodes); diff --git a/src/engine/routing_algorithms/map_matching.cpp b/src/engine/routing_algorithms/map_matching.cpp index 3bbf96c22..9f8a1d2a5 100644 --- a/src/engine/routing_algorithms/map_matching.cpp +++ b/src/engine/routing_algorithms/map_matching.cpp @@ -49,7 +49,7 @@ inline void initializeHeap(SearchEngineData &engine_working_data, { const auto nodes_number = facade.GetNumberOfNodes(); - engine_working_data.InitializeOrClearFirstThreadLocalStorage(nodes_number); + engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(nodes_number); } template <> @@ -59,7 +59,8 @@ inline void initializeHeap(SearchEngineData &eng const auto nodes_number = facade.GetNumberOfNodes(); const auto border_nodes_number = facade.GetMaxBorderNodeID() + 1; - engine_working_data.InitializeOrClearFirstThreadLocalStorage(nodes_number, border_nodes_number); + engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(nodes_number, + border_nodes_number); } } // namespace @@ -144,8 +145,8 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, } initializeHeap(engine_working_data, facade); - auto &forward_heap = *engine_working_data.forward_heap_1; - auto &reverse_heap = *engine_working_data.reverse_heap_1; + auto &forward_heap = *engine_working_data.map_matching_forward_heap_1; + auto &reverse_heap = *engine_working_data.map_matching_reverse_heap_1; std::size_t breakage_begin = map_matching::INVALID_STATE; std::vector split_points; diff --git a/src/engine/search_engine_data.cpp b/src/engine/search_engine_data.cpp index a4a2bc5dd..dd1a053e0 100644 --- a/src/engine/search_engine_data.cpp +++ b/src/engine/search_engine_data.cpp @@ -11,8 +11,32 @@ SearchEngineData::SearchEngineHeapPtr SearchEngineData::forward_heap_2; SearchEngineData::SearchEngineHeapPtr SearchEngineData::reverse_heap_2; SearchEngineData::SearchEngineHeapPtr SearchEngineData::forward_heap_3; SearchEngineData::SearchEngineHeapPtr SearchEngineData::reverse_heap_3; +SearchEngineData::SearchEngineHeapPtr SearchEngineData::map_matching_forward_heap_1; +SearchEngineData::SearchEngineHeapPtr SearchEngineData::map_matching_reverse_heap_1; + SearchEngineData::ManyToManyHeapPtr SearchEngineData::many_to_many_heap; +void SearchEngineData::InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes) +{ + if (map_matching_forward_heap_1.get()) + { + map_matching_forward_heap_1->Clear(); + } + else + { + map_matching_forward_heap_1.reset(new QueryHeap(number_of_nodes)); + } + + if (map_matching_reverse_heap_1.get()) + { + map_matching_reverse_heap_1->Clear(); + } + else + { + map_matching_reverse_heap_1.reset(new QueryHeap(number_of_nodes)); + } +} + void SearchEngineData::InitializeOrClearFirstThreadLocalStorage(unsigned number_of_nodes) { if (forward_heap_1.get()) @@ -92,8 +116,34 @@ void SearchEngineData::InitializeOrClearManyToManyThreadLocalStorage(unsigne using MLD = routing_algorithms::mld::Algorithm; SearchEngineData::SearchEngineHeapPtr SearchEngineData::forward_heap_1; SearchEngineData::SearchEngineHeapPtr SearchEngineData::reverse_heap_1; +SearchEngineData::MapMatchingHeapPtr SearchEngineData::map_matching_forward_heap_1; +SearchEngineData::MapMatchingHeapPtr SearchEngineData::map_matching_reverse_heap_1; SearchEngineData::ManyToManyHeapPtr SearchEngineData::many_to_many_heap; +void SearchEngineData::InitializeOrClearMapMatchingThreadLocalStorage( + unsigned number_of_nodes, unsigned number_of_boundary_nodes) +{ + if (map_matching_forward_heap_1.get()) + { + map_matching_forward_heap_1->Clear(); + } + else + { + map_matching_forward_heap_1.reset( + new MapMatchingQueryHeap(number_of_nodes, number_of_boundary_nodes)); + } + + if (map_matching_reverse_heap_1.get()) + { + map_matching_reverse_heap_1->Clear(); + } + else + { + map_matching_reverse_heap_1.reset( + new MapMatchingQueryHeap(number_of_nodes, number_of_boundary_nodes)); + } +} + void SearchEngineData::InitializeOrClearFirstThreadLocalStorage( unsigned number_of_nodes, unsigned number_of_boundary_nodes) {