From 96c64eeca19dee43287083d154e92d560cf9c8be Mon Sep 17 00:00:00 2001 From: Michael Krasnyk Date: Mon, 6 Mar 2017 20:16:24 +0100 Subject: [PATCH] MLD plugin --- include/engine/algorithm.hpp | 2 +- include/engine/routing_algorithms.hpp | 7 - .../direct_shortest_path.hpp | 14 +- .../routing_algorithms/routing_base_ch.hpp | 17 -- .../routing_algorithms/routing_base_mld.hpp | 276 ++++++++++++++++++ include/engine/search_engine_data.hpp | 30 ++ .../direct_shortest_path.cpp | 93 +++++- src/engine/search_engine_data.cpp | 25 ++ 8 files changed, 415 insertions(+), 49 deletions(-) create mode 100644 include/engine/routing_algorithms/routing_base_mld.hpp diff --git a/include/engine/algorithm.hpp b/include/engine/algorithm.hpp index d6a8dd337e0..d106d9b2133 100644 --- a/include/engine/algorithm.hpp +++ b/include/engine/algorithm.hpp @@ -100,7 +100,7 @@ template <> struct HasManyToManySearch final : std::false_type template <> struct HasShortestPathSearch final : std::false_type { }; -template <> struct HasDirectShortestPathSearch final : std::false_type +template <> struct HasDirectShortestPathSearch final : std::true_type { }; template <> struct HasMapMatching final : std::false_type diff --git a/include/engine/routing_algorithms.hpp b/include/engine/routing_algorithms.hpp index 5b9fd553ed4..eefe068333f 100644 --- a/include/engine/routing_algorithms.hpp +++ b/include/engine/routing_algorithms.hpp @@ -193,13 +193,6 @@ RoutingAlgorithms::ShortestPathSearch(const std::vector -InternalRouteResult inline RoutingAlgorithms::DirectShortestPathSearch( - const PhantomNodes &) const -{ - throw util::exception("DirectShortestPathSearch is not implemented"); -} - template <> inline std::vector RoutingAlgorithms::ManyToManySearch(const std::vector &, diff --git a/include/engine/routing_algorithms/direct_shortest_path.hpp b/include/engine/routing_algorithms/direct_shortest_path.hpp index 5b1be5df72b..3a9472314c9 100644 --- a/include/engine/routing_algorithms/direct_shortest_path.hpp +++ b/include/engine/routing_algorithms/direct_shortest_path.hpp @@ -21,15 +21,11 @@ namespace routing_algorithms /// by the previous route. /// This variation is only an optimazation for graphs with slow queries, for example /// not fully contracted graphs. -InternalRouteResult directShortestPathSearch( - SearchEngineData &engine_working_data, - const datafacade::ContiguousInternalMemoryDataFacade &facade, - const PhantomNodes &phantom_nodes); - -InternalRouteResult directShortestPathSearch( - SearchEngineData &engine_working_data, - const datafacade::ContiguousInternalMemoryDataFacade &facade, - const PhantomNodes &phantom_nodes); +template +InternalRouteResult +directShortestPathSearch(SearchEngineData &engine_working_data, + const datafacade::ContiguousInternalMemoryDataFacade &facade, + const PhantomNodes &phantom_nodes); } // namespace routing_algorithms } // namespace engine diff --git a/include/engine/routing_algorithms/routing_base_ch.hpp b/include/engine/routing_algorithms/routing_base_ch.hpp index 994e5e5c901..e6bf5682c5d 100644 --- a/include/engine/routing_algorithms/routing_base_ch.hpp +++ b/include/engine/routing_algorithms/routing_base_ch.hpp @@ -1,32 +1,15 @@ #ifndef OSRM_ENGINE_ROUTING_BASE_CH_HPP #define OSRM_ENGINE_ROUTING_BASE_CH_HPP -#include "extractor/guidance/turn_instruction.hpp" - #include "engine/algorithm.hpp" #include "engine/datafacade/contiguous_internalmem_datafacade.hpp" -#include "engine/internal_route_result.hpp" #include "engine/routing_algorithms/routing_base.hpp" #include "engine/search_engine_data.hpp" -#include "util/coordinate_calculation.hpp" -#include "util/guidance/turn_bearing.hpp" #include "util/typedefs.hpp" #include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - namespace osrm { namespace engine diff --git a/include/engine/routing_algorithms/routing_base_mld.hpp b/include/engine/routing_algorithms/routing_base_mld.hpp new file mode 100644 index 00000000000..eb639c81489 --- /dev/null +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -0,0 +1,276 @@ +#ifndef OSRM_ENGINE_ROUTING_BASE_MLD_HPP +#define OSRM_ENGINE_ROUTING_BASE_MLD_HPP + +#include "engine/algorithm.hpp" +#include "engine/datafacade/contiguous_internalmem_datafacade.hpp" +#include "engine/routing_algorithms/routing_base.hpp" +#include "engine/search_engine_data.hpp" + +#include "util/typedefs.hpp" + +#include + +namespace osrm +{ +namespace engine +{ +namespace routing_algorithms +{ +namespace mld +{ + +template +void routingStep(const datafacade::ContiguousInternalMemoryDataFacade &facade, + const partition::MultiLevelPartitionView &partition, + const partition::CellStorageView &cells, + SearchEngineData::MultiLayerDijkstraHeap &forward_heap, + SearchEngineData::MultiLayerDijkstraHeap &reverse_heap, + const std::pair &parent_cell, + NodeID &middle_node, + EdgeWeight &path_upper_bound, + EdgeWeight &forward_upper_bound, + EdgeWeight &reverse_upper_bound) +{ + const auto node = forward_heap.DeleteMin(); + const auto weight = forward_heap.GetKey(node); + + auto update_upper_bounds = [&](NodeID to, EdgeWeight forward_weight, EdgeWeight edge_weight) { + if (reverse_heap.WasInserted(to) && reverse_heap.WasRemoved(to)) + { + auto reverse_weight = reverse_heap.GetKey(to); + auto path_weight = forward_weight + edge_weight + reverse_weight; + BOOST_ASSERT(path_weight >= 0); + if (path_weight < path_upper_bound) + { + middle_node = to; + path_upper_bound = path_weight; + forward_upper_bound = forward_weight + edge_weight; + reverse_upper_bound = reverse_weight + edge_weight; + } + } + }; + + // Edge case: single node path + update_upper_bounds(node, weight, 0); + + const auto &node_data = forward_heap.GetData(node); + const auto is_boundary = node_data.IsBoundary() || node_data.parent == node; + const auto level = node_data.level; + + if (is_boundary && level >= 1) + { + if (DIRECTION == FORWARD_DIRECTION) + { + // Shortcuts in forward direction + const auto &cell = cells.GetCell(level, partition.GetCell(level, node)); + auto destination = cell.GetDestinationNodes().begin(); + for (auto shortcut_weight : cell.GetOutWeight(node)) + { + BOOST_ASSERT(destination != cell.GetDestinationNodes().end()); + const NodeID to = *destination; + if (shortcut_weight != INVALID_EDGE_WEIGHT && node != to) + { + const EdgeWeight to_weight = weight + shortcut_weight; + if (!forward_heap.WasInserted(to)) + { + forward_heap.Insert(to, to_weight, {node, level}); + update_upper_bounds(to, weight, shortcut_weight); + } + else if (to_weight < forward_heap.GetKey(to)) + { + forward_heap.GetData(to) = {node, level}; + forward_heap.DecreaseKey(to, to_weight); + update_upper_bounds(to, weight, shortcut_weight); + } + } + ++destination; + } + } + else + { + // Shortcuts in backward direction + const auto &cell = cells.GetCell(level, partition.GetCell(level, node)); + auto source = cell.GetSourceNodes().begin(); + for (auto shortcut_weight : cell.GetInWeight(node)) + { + BOOST_ASSERT(source != cell.GetSourceNodes().end()); + const NodeID to = *source; + if (shortcut_weight != INVALID_EDGE_WEIGHT && node != to) + { + const EdgeWeight to_weight = weight + shortcut_weight; + if (!forward_heap.WasInserted(to)) + { + forward_heap.Insert(to, to_weight, {node, level}); + update_upper_bounds(to, weight, shortcut_weight); + } + else if (to_weight < forward_heap.GetKey(to)) + { + forward_heap.GetData(to) = {node, level}; + forward_heap.DecreaseKey(to, to_weight); + update_upper_bounds(to, weight, shortcut_weight); + } + } + ++source; + } + } + } + + // Boundary edges + for (const auto edge : facade.GetAdjacentEdgeRange(node)) + { + const auto &edge_data = facade.GetEdgeData(edge); + if (DIRECTION == FORWARD_DIRECTION ? edge_data.forward : edge_data.backward) + { + const NodeID to = facade.GetTarget(edge); + const auto to_level = + std::min(parent_cell.first, partition.GetHighestDifferentLevel(node, to)); + + if ( // Routing is unrestricted or restricted to the highest level cell + (parent_cell.second == INVALID_CELL_ID || + parent_cell.second == partition.GetCell(parent_cell.first + 1, to)) && + // "Never-go-down" at border edges + to_level >= level) + { + BOOST_ASSERT_MSG(edge_data.weight > 0, "edge_weight invalid"); + const EdgeWeight to_weight = weight + edge_data.weight; + + if (!forward_heap.WasInserted(to)) + { + forward_heap.Insert(to, to_weight, {node, to_level, edge}); + update_upper_bounds(to, weight, edge_data.weight); + } + else if (to_weight < forward_heap.GetKey(to)) + { + forward_heap.GetData(to) = {node, to_level, edge}; + forward_heap.DecreaseKey(to, to_weight); + update_upper_bounds(to, weight, edge_data.weight); + } + } + } + } +} + +auto search(const datafacade::ContiguousInternalMemoryDataFacade &facade, + const partition::MultiLevelPartitionView &partition, + const partition::CellStorageView &cells, + SearchEngineData::MultiLayerDijkstraHeap &forward_heap, + SearchEngineData::MultiLayerDijkstraHeap &reverse_heap, + const std::pair &parent_cell) +{ + // run two-Target Dijkstra routing step. + NodeID middle = SPECIAL_NODEID; + EdgeWeight weight = INVALID_EDGE_WEIGHT; + EdgeWeight forward_search_radius = INVALID_EDGE_WEIGHT; + EdgeWeight reverse_search_radius = INVALID_EDGE_WEIGHT; + bool progress; + do + { + progress = false; + if (!forward_heap.Empty() && (forward_heap.MinKey() < forward_search_radius)) + { + progress = true; + routingStep(facade, + partition, + cells, + forward_heap, + reverse_heap, + parent_cell, + middle, + weight, + forward_search_radius, + reverse_search_radius); + } + if (!reverse_heap.Empty() && (reverse_heap.MinKey() < reverse_search_radius)) + { + progress = true; + routingStep(facade, + partition, + cells, + reverse_heap, + forward_heap, + parent_cell, + middle, + weight, + reverse_search_radius, + forward_search_radius); + } + } while (progress); + + // No path found for both target nodes? + if (weight == INVALID_EDGE_WEIGHT || SPECIAL_NODEID == middle) + { + return std::make_tuple( + INVALID_EDGE_WEIGHT, SPECIAL_NODEID, SPECIAL_NODEID, std::vector()); + } + + // Get packed path as edges {level, from node ID, to node ID, edge ID} + std::vector> packed_path; + NodeID current_node = middle, parent_node = forward_heap.GetData(middle).parent; + while (parent_node != current_node) + { + const auto &data = forward_heap.GetData(current_node); + packed_path.push_back({data.level, parent_node, current_node, data.edge_id}); + current_node = parent_node; + parent_node = forward_heap.GetData(parent_node).parent; + } + std::reverse(std::begin(packed_path), std::end(packed_path)); + const NodeID source_node = current_node; + + current_node = middle, parent_node = reverse_heap.GetData(middle).parent; + while (parent_node != current_node) + { + const auto &data = reverse_heap.GetData(current_node); + packed_path.push_back({data.level, current_node, parent_node, data.edge_id}); + current_node = parent_node; + parent_node = reverse_heap.GetData(parent_node).parent; + } + const NodeID target_node = current_node; + + // Unpack path + std::vector unpacked_path; + unpacked_path.reserve(packed_path.size()); + for (auto &packed_edge : packed_path) + { + LevelID level; + NodeID source, target; + EdgeID edge_id; + std::tie(level, source, target, edge_id) = packed_edge; + if (edge_id != SPECIAL_EDGEID) + { // a base graph edge + unpacked_path.push_back(edge_id); + } + else + { // an overlay graph edge + LevelID sublevel = level - 1; + CellID parent_cell_id = partition.GetCell(level, source); + BOOST_ASSERT(parent_cell_id == partition.GetCell(level, target)); + + // Here heaps can be reused, let's go deeper! + forward_heap.Clear(); + reverse_heap.Clear(); + forward_heap.Insert(source, 0, {source, sublevel}); + reverse_heap.Insert(target, 0, {target, sublevel}); + + // TODO: when structured bindings will be allowed change to + // auto [subpath_weight, subpath_source, subpath_target, subpath] = ... + EdgeWeight subpath_weight; + NodeID subpath_source, subpath_target; + std::vector subpath; + std::tie(subpath_weight, subpath_source, subpath_target, subpath) = search( + facade, partition, cells, forward_heap, reverse_heap, {sublevel, parent_cell_id}); + BOOST_ASSERT(!subpath.empty()); + BOOST_ASSERT(subpath_source == source); + BOOST_ASSERT(subpath_target == target); + unpacked_path.insert(unpacked_path.end(), subpath.begin(), subpath.end()); + } + } + + return std::make_tuple(weight, source_node, target_node, std::move(unpacked_path)); +} + +} // namespace mld +} // namespace routing_algorithms +} // namespace engine +} // namespace osrm + +#endif // OSRM_ENGINE_ROUTING_BASE_MLD_HPP diff --git a/include/engine/search_engine_data.hpp b/include/engine/search_engine_data.hpp index ba57487b3cc..4a7633af8e1 100644 --- a/include/engine/search_engine_data.hpp +++ b/include/engine/search_engine_data.hpp @@ -3,6 +3,7 @@ #include +#include "partition/multi_level_partition.hpp" #include "util/binary_heap.hpp" #include "util/typedefs.hpp" @@ -23,6 +24,23 @@ struct ManyToManyHeapData : HeapData ManyToManyHeapData(NodeID p, EdgeWeight duration) : HeapData(p), duration(duration) {} }; +struct MultiLayerDijkstraHeapData : HeapData +{ + LevelID level; // node level: always increasing along the path starting from 0 + EdgeID edge_id; // edge id if parent -> node is a boundary edge + MultiLayerDijkstraHeapData(NodeID p) : HeapData(p), level(0), edge_id(SPECIAL_EDGEID) {} + MultiLayerDijkstraHeapData(NodeID p, LevelID level) + : HeapData(p), level(level), edge_id(SPECIAL_EDGEID) + { + } + MultiLayerDijkstraHeapData(NodeID p, LevelID level, EdgeID edge_id) + : HeapData(p), level(level), edge_id(edge_id) + { + } + + auto IsBoundary() const { return edge_id != SPECIAL_EDGEID; } +}; + struct SearchEngineData { using QueryHeap = util:: @@ -37,6 +55,14 @@ struct SearchEngineData using ManyToManyHeapPtr = boost::thread_specific_ptr; + using MultiLayerDijkstraHeap = util::BinaryHeap>; + + using MultiLayerDijkstraHeapPtr = boost::thread_specific_ptr; + static SearchEngineHeapPtr forward_heap_1; static SearchEngineHeapPtr reverse_heap_1; static SearchEngineHeapPtr forward_heap_2; @@ -44,6 +70,8 @@ struct SearchEngineData static SearchEngineHeapPtr forward_heap_3; static SearchEngineHeapPtr reverse_heap_3; static ManyToManyHeapPtr many_to_many_heap; + static MultiLayerDijkstraHeapPtr mld_forward_heap; + static MultiLayerDijkstraHeapPtr mld_reverse_heap; void InitializeOrClearFirstThreadLocalStorage(const unsigned number_of_nodes); @@ -52,6 +80,8 @@ struct SearchEngineData void InitializeOrClearThirdThreadLocalStorage(const unsigned number_of_nodes); void InitializeOrClearManyToManyThreadLocalStorage(const unsigned number_of_nodes); + + void InitializeOrClearMultiLayerDijkstraThreadLocalStorage(const unsigned number_of_nodes); }; } } diff --git a/src/engine/routing_algorithms/direct_shortest_path.cpp b/src/engine/routing_algorithms/direct_shortest_path.cpp index efcbd593117..887905469ef 100644 --- a/src/engine/routing_algorithms/direct_shortest_path.cpp +++ b/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 InternalRouteResult extractRoute(const datafacade::ContiguousInternalMemoryDataFacade &facade, const EdgeWeight weight, - const std::vector &packed_leg, + const NodeID source_node, + const NodeID target_node, + const std::vector &edges, const PhantomNodes &nodes) { InternalRouteResult raw_route_data; @@ -30,24 +30,25 @@ extractRoute(const datafacade::ContiguousInternalMemoryDataFacade &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 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 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 & /* 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 &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 &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 &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 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 diff --git a/src/engine/search_engine_data.cpp b/src/engine/search_engine_data.cpp index 644e5ba68ad..9e299fc997d 100644 --- a/src/engine/search_engine_data.cpp +++ b/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)); + } +} } }