diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index 946c506f443..def1960e658 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -1,5 +1,5 @@ -#ifndef ROUTING_BASE_HPP -#define ROUTING_BASE_HPP +#ifndef OSRM_ENGINE_ROUTING_BASE_HPP +#define OSRM_ENGINE_ROUTING_BASE_HPP #include "extractor/guidance/turn_instruction.hpp" @@ -35,381 +35,150 @@ namespace routing_algorithms { static constexpr bool FORWARD_DIRECTION = true; static constexpr bool REVERSE_DIRECTION = false; +static constexpr bool DO_NOT_FORCE_LOOPS = false; -// Stalling -template -bool stallAtNode(const datafacade::ContiguousInternalMemoryDataFacade &facade, - const NodeID node, - const EdgeWeight weight, - const HeapT &query_heap) +template +void insertNodesInHeap(Heap &heap, const PhantomNode &phantom_node) { - for (auto edge : facade.GetAdjacentEdgeRange(node)) + BOOST_ASSERT(phantom_node.IsValid()); + + const auto weight_sign = DIRECTION == FORWARD_DIRECTION ? -1 : 1; + if (phantom_node.forward_segment_id.enabled) { - const auto &data = facade.GetEdgeData(edge); - if (DIRECTION == REVERSE_DIRECTION ? data.forward : data.backward) - { - const NodeID to = facade.GetTarget(edge); - const EdgeWeight edge_weight = data.weight; - BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid"); - if (query_heap.WasInserted(to)) - { - if (query_heap.GetKey(to) + edge_weight < weight) - { - return true; - } - } - } + heap.Insert(phantom_node.forward_segment_id.id, + weight_sign * phantom_node.GetForwardWeightPlusOffset(), + phantom_node.forward_segment_id.id); } - return false; -} - -template -void relaxOutgoingEdges(const datafacade::ContiguousInternalMemoryDataFacade &facade, - const NodeID node, - const EdgeWeight weight, - SearchEngineData::QueryHeap &heap) -{ - for (const auto edge : facade.GetAdjacentEdgeRange(node)) + if (phantom_node.reverse_segment_id.enabled) { - const auto &data = facade.GetEdgeData(edge); - if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward) - { - const NodeID to = facade.GetTarget(edge); - const EdgeWeight edge_weight = data.weight; - - BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid"); - const EdgeWeight to_weight = weight + edge_weight; - - // New Node discovered -> Add to Heap + Node Info Storage - if (!heap.WasInserted(to)) - { - heap.Insert(to, to_weight, node); - } - // Found a shorter Path -> Update weight - else if (to_weight < heap.GetKey(to)) - { - // new parent - heap.GetData(to).parent = node; - heap.DecreaseKey(to, to_weight); - } - } + heap.Insert(phantom_node.reverse_segment_id.id, + weight_sign * phantom_node.GetReverseWeightPlusOffset(), + phantom_node.reverse_segment_id.id); } } -/* -min_edge_offset is needed in case we use multiple -nodes as start/target nodes with different (even negative) offsets. -In that case the termination criterion is not correct -anymore. - -Example: -forward heap: a(-100), b(0), -reverse heap: c(0), d(100) - -a --- d - \ / - / \ -b --- c - -This is equivalent to running a bi-directional Dijkstra on the following graph: - - a --- d - / \ / \ - y x z - \ / \ / - b --- c - -The graph is constructed by inserting nodes y and z that are connected to the initial nodes -using edges (y, a) with weight -100, (y, b) with weight 0 and, -(d, z) with weight 100, (c, z) with weight 0 corresponding. -Since we are dealing with a graph that contains _negative_ edges, -we need to add an offset to the termination criterion. -*/ -static constexpr bool ENABLE_STALLING = true; -static constexpr bool DISABLE_STALLING = false; -static constexpr bool DO_NOT_FORCE_LOOPS = false; -template -void routingStep(const datafacade::ContiguousInternalMemoryDataFacade &facade, - SearchEngineData::QueryHeap &forward_heap, - SearchEngineData::QueryHeap &reverse_heap, - NodeID &middle_node_id, - EdgeWeight &upper_bound, - EdgeWeight min_edge_offset, - const bool force_loop_forward, - const bool force_loop_reverse) +template +void insertNodesInHeap(SearchEngineData::ManyToManyQueryHeap &heap, const PhantomNode &phantom_node) { - const NodeID node = forward_heap.DeleteMin(); - const EdgeWeight weight = forward_heap.GetKey(node); + BOOST_ASSERT(phantom_node.IsValid()); - if (reverse_heap.WasInserted(node)) + const auto weight_sign = DIRECTION == FORWARD_DIRECTION ? -1 : 1; + if (phantom_node.forward_segment_id.enabled) { - const EdgeWeight new_weight = reverse_heap.GetKey(node) + weight; - if (new_weight < upper_bound) - { - // if loops are forced, they are so at the source - if ((force_loop_forward && forward_heap.GetData(node).parent == node) || - (force_loop_reverse && reverse_heap.GetData(node).parent == node) || - // in this case we are looking at a bi-directional way where the source - // and target phantom are on the same edge based node - new_weight < 0) - { - // check whether there is a loop present at the node - for (const auto edge : facade.GetAdjacentEdgeRange(node)) - { - const auto &data = facade.GetEdgeData(edge); - if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward) - { - const NodeID to = facade.GetTarget(edge); - if (to == node) - { - const EdgeWeight edge_weight = data.weight; - const EdgeWeight loop_weight = new_weight + edge_weight; - if (loop_weight >= 0 && loop_weight < upper_bound) - { - middle_node_id = node; - upper_bound = loop_weight; - } - } - } - } - } - else - { - BOOST_ASSERT(new_weight >= 0); - - middle_node_id = node; - upper_bound = new_weight; - } - } - } - - // make sure we don't terminate too early if we initialize the weight - // for the nodes in the forward heap with the forward/reverse offset - BOOST_ASSERT(min_edge_offset <= 0); - if (weight + min_edge_offset > upper_bound) - { - forward_heap.DeleteAll(); - return; + heap.Insert( + phantom_node.forward_segment_id.id, + weight_sign * phantom_node.GetForwardWeightPlusOffset(), + {phantom_node.forward_segment_id.id, weight_sign * phantom_node.GetForwardDuration()}); } - - // Stalling - if (STALLING && stallAtNode(facade, node, weight, forward_heap)) + if (phantom_node.reverse_segment_id.enabled) { - return; + heap.Insert( + phantom_node.reverse_segment_id.id, + weight_sign * phantom_node.GetReverseWeightPlusOffset(), + {phantom_node.reverse_segment_id.id, weight_sign * phantom_node.GetReverseDuration()}); } - - relaxOutgoingEdges(facade, node, weight, forward_heap); } -template -EdgeWeight -getLoopWeight(const datafacade::ContiguousInternalMemoryDataFacade &facade, - NodeID node) +template +void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes) { - EdgeWeight loop_weight = UseDuration ? MAXIMAL_EDGE_DURATION : INVALID_EDGE_WEIGHT; - for (auto edge : facade.GetAdjacentEdgeRange(node)) - { - const auto &data = facade.GetEdgeData(edge); - if (data.forward) - { - const NodeID to = facade.GetTarget(edge); - if (to == node) - { - const auto value = UseDuration ? data.duration : data.weight; - loop_weight = std::min(loop_weight, value); - } - } - } - return loop_weight; + insertNodesInHeap(forward_heap, nodes.source_phantom); + insertNodesInHeap(reverse_heap, nodes.target_phantom); } -/** - * Given a sequence of connected `NodeID`s in the CH graph, performs a depth-first unpacking of - * the shortcut - * edges. For every "original" edge found, it calls the `callback` with the two NodeIDs for the - * edge, and the EdgeData - * for that edge. - * - * The primary purpose of this unpacking is to expand a path through the CH into the original - * route through the - * pre-contracted graph. - * - * Because of the depth-first-search, the `callback` will effectively be called in sequence for - * the original route - * from beginning to end. - * - * @param packed_path_begin iterator pointing to the start of the NodeID list - * @param packed_path_end iterator pointing to the end of the NodeID list - * @param callback void(const std::pair, const EdgeData &) called for each - * original edge found. - */ -template -void unpackPath(const datafacade::ContiguousInternalMemoryDataFacade &facade, - BidirectionalIterator packed_path_begin, - BidirectionalIterator packed_path_end, - Callback &&callback) +template +void annotatePath(const FacadeT &facade, + const NodeID source_node, + const NodeID target_node, + const std::vector &unpacked_edges, + const PhantomNodes &phantom_node_pair, + std::vector &unpacked_path) { - // make sure we have at least something to unpack - if (packed_path_begin == packed_path_end) - return; + BOOST_ASSERT(source_node != SPECIAL_NODEID && target_node != SPECIAL_NODEID); + BOOST_ASSERT(!unpacked_edges.empty() || source_node == target_node); - std::stack> recursion_stack; + const bool start_traversed_in_reverse = + phantom_node_pair.source_phantom.forward_segment_id.id != source_node; + const bool target_traversed_in_reverse = + phantom_node_pair.target_phantom.forward_segment_id.id != target_node; - // We have to push the path in reverse order onto the stack because it's LIFO. - for (auto current = std::prev(packed_path_end); current != packed_path_begin; - current = std::prev(current)) - { - recursion_stack.emplace(*std::prev(current), *current); - } + BOOST_ASSERT(phantom_node_pair.source_phantom.forward_segment_id.id == source_node || + phantom_node_pair.source_phantom.reverse_segment_id.id == source_node); + BOOST_ASSERT(phantom_node_pair.target_phantom.forward_segment_id.id == target_node || + phantom_node_pair.target_phantom.reverse_segment_id.id == target_node); - std::pair edge; - while (!recursion_stack.empty()) + for (auto edge_id : unpacked_edges) { - edge = recursion_stack.top(); - recursion_stack.pop(); - - // Look for an edge on the forward CH graph (.forward) - EdgeID smaller_edge_id = facade.FindSmallestEdge( - edge.first, edge.second, [](const auto &data) { return data.forward; }); - - // If we didn't find one there, the we might be looking at a part of the path that - // was found using the backward search. Here, we flip the node order (.second, .first) - // and only consider edges with the `.backward` flag. - if (SPECIAL_EDGEID == smaller_edge_id) + const auto &edge_data = facade.GetEdgeData(edge_id); + const auto turn_id = edge_data.turn_id; // edge-based node ID + const auto name_index = facade.GetNameIndexFromEdgeID(turn_id); + const auto turn_instruction = facade.GetTurnInstructionForEdgeID(turn_id); + const extractor::TravelMode travel_mode = + (unpacked_path.empty() && start_traversed_in_reverse) + ? phantom_node_pair.source_phantom.backward_travel_mode + : facade.GetTravelModeForEdgeID(turn_id); + + const auto geometry_index = facade.GetGeometryIndexForEdgeID(turn_id); + std::vector id_vector; + + std::vector weight_vector; + std::vector duration_vector; + std::vector datasource_vector; + if (geometry_index.forward) { - smaller_edge_id = facade.FindSmallestEdge( - edge.second, edge.first, [](const auto &data) { return data.backward; }); - } - - // If we didn't find anything *still*, then something is broken and someone has - // called this function with bad values. - BOOST_ASSERT_MSG(smaller_edge_id != SPECIAL_EDGEID, "Invalid smaller edge ID"); - - const auto &data = facade.GetEdgeData(smaller_edge_id); - BOOST_ASSERT_MSG(data.weight != std::numeric_limits::max(), - "edge weight invalid"); - - // If the edge is a shortcut, we need to add the two halfs to the stack. - if (data.shortcut) - { // unpack - const NodeID middle_node_id = data.turn_id; - // Note the order here - we're adding these to a stack, so we - // want the first->middle to get visited before middle->second - recursion_stack.emplace(middle_node_id, edge.second); - recursion_stack.emplace(edge.first, middle_node_id); + id_vector = facade.GetUncompressedForwardGeometry(geometry_index.id); + weight_vector = facade.GetUncompressedForwardWeights(geometry_index.id); + duration_vector = facade.GetUncompressedForwardDurations(geometry_index.id); + datasource_vector = facade.GetUncompressedForwardDatasources(geometry_index.id); } else { - // We found an original edge, call our callback. - std::forward(callback)(edge, data); + id_vector = facade.GetUncompressedReverseGeometry(geometry_index.id); + weight_vector = facade.GetUncompressedReverseWeights(geometry_index.id); + duration_vector = facade.GetUncompressedReverseDurations(geometry_index.id); + datasource_vector = facade.GetUncompressedReverseDatasources(geometry_index.id); } + BOOST_ASSERT(id_vector.size() > 0); + BOOST_ASSERT(datasource_vector.size() > 0); + BOOST_ASSERT(weight_vector.size() == id_vector.size() - 1); + BOOST_ASSERT(duration_vector.size() == id_vector.size() - 1); + const bool is_first_segment = unpacked_path.empty(); + + const std::size_t start_index = + (is_first_segment ? ((start_traversed_in_reverse) + ? weight_vector.size() - + phantom_node_pair.source_phantom.fwd_segment_position - 1 + : phantom_node_pair.source_phantom.fwd_segment_position) + : 0); + const std::size_t end_index = weight_vector.size(); + + BOOST_ASSERT(start_index >= 0); + BOOST_ASSERT(start_index < end_index); + for (std::size_t segment_idx = start_index; segment_idx < end_index; ++segment_idx) + { + unpacked_path.push_back(PathData{id_vector[segment_idx + 1], + name_index, + weight_vector[segment_idx], + duration_vector[segment_idx], + extractor::guidance::TurnInstruction::NO_TURN(), + {{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID}, + travel_mode, + INVALID_ENTRY_CLASSID, + datasource_vector[segment_idx], + util::guidance::TurnBearing(0), + util::guidance::TurnBearing(0)}); + } + BOOST_ASSERT(unpacked_path.size() > 0); + if (facade.hasLaneData(turn_id)) + unpacked_path.back().lane_data = facade.GetLaneData(turn_id); + + unpacked_path.back().entry_classid = facade.GetEntryClassID(turn_id); + unpacked_path.back().turn_instruction = turn_instruction; + unpacked_path.back().duration_until_turn += facade.GetDurationPenaltyForEdgeID(turn_id); + unpacked_path.back().weight_until_turn += facade.GetWeightPenaltyForEdgeID(turn_id); + unpacked_path.back().pre_turn_bearing = facade.PreTurnBearing(turn_id); + unpacked_path.back().post_turn_bearing = facade.PostTurnBearing(turn_id); } -} - -// Should work both for CH and not CH if the unpackPath function above is implemented a proper -// implementation. -template -void unpackPath(const FacadeT &facade, - RandomIter packed_path_begin, - RandomIter packed_path_end, - const PhantomNodes &phantom_node_pair, - std::vector &unpacked_path) -{ - BOOST_ASSERT(std::distance(packed_path_begin, packed_path_end) > 0); - - const bool start_traversed_in_reverse = - (*packed_path_begin != phantom_node_pair.source_phantom.forward_segment_id.id); - const bool target_traversed_in_reverse = - (*std::prev(packed_path_end) != phantom_node_pair.target_phantom.forward_segment_id.id); - - BOOST_ASSERT(*packed_path_begin == phantom_node_pair.source_phantom.forward_segment_id.id || - *packed_path_begin == phantom_node_pair.source_phantom.reverse_segment_id.id); - BOOST_ASSERT( - *std::prev(packed_path_end) == phantom_node_pair.target_phantom.forward_segment_id.id || - *std::prev(packed_path_end) == phantom_node_pair.target_phantom.reverse_segment_id.id); - - unpackPath( - facade, - packed_path_begin, - packed_path_end, - [&facade, - &unpacked_path, - &phantom_node_pair, - &start_traversed_in_reverse, - &target_traversed_in_reverse](std::pair & /* edge */, - const auto &edge_data) { - - BOOST_ASSERT_MSG(!edge_data.shortcut, "original edge flagged as shortcut"); - const auto turn_id = edge_data.turn_id; // edge-based node ID - const auto name_index = facade.GetNameIndexFromEdgeID(turn_id); - const auto turn_instruction = facade.GetTurnInstructionForEdgeID(turn_id); - const extractor::TravelMode travel_mode = - (unpacked_path.empty() && start_traversed_in_reverse) - ? phantom_node_pair.source_phantom.backward_travel_mode - : facade.GetTravelModeForEdgeID(turn_id); - - const auto geometry_index = facade.GetGeometryIndexForEdgeID(turn_id); - std::vector id_vector; - - std::vector weight_vector; - std::vector duration_vector; - std::vector datasource_vector; - if (geometry_index.forward) - { - id_vector = facade.GetUncompressedForwardGeometry(geometry_index.id); - weight_vector = facade.GetUncompressedForwardWeights(geometry_index.id); - duration_vector = facade.GetUncompressedForwardDurations(geometry_index.id); - datasource_vector = facade.GetUncompressedForwardDatasources(geometry_index.id); - } - else - { - id_vector = facade.GetUncompressedReverseGeometry(geometry_index.id); - weight_vector = facade.GetUncompressedReverseWeights(geometry_index.id); - duration_vector = facade.GetUncompressedReverseDurations(geometry_index.id); - datasource_vector = facade.GetUncompressedReverseDatasources(geometry_index.id); - } - BOOST_ASSERT(id_vector.size() > 0); - BOOST_ASSERT(datasource_vector.size() > 0); - BOOST_ASSERT(weight_vector.size() == id_vector.size() - 1); - BOOST_ASSERT(duration_vector.size() == id_vector.size() - 1); - const bool is_first_segment = unpacked_path.empty(); - - const std::size_t start_index = - (is_first_segment - ? ((start_traversed_in_reverse) - ? weight_vector.size() - - phantom_node_pair.source_phantom.fwd_segment_position - 1 - : phantom_node_pair.source_phantom.fwd_segment_position) - : 0); - const std::size_t end_index = weight_vector.size(); - - BOOST_ASSERT(start_index >= 0); - BOOST_ASSERT(start_index < end_index); - for (std::size_t segment_idx = start_index; segment_idx < end_index; ++segment_idx) - { - unpacked_path.push_back(PathData{id_vector[segment_idx + 1], - name_index, - weight_vector[segment_idx], - duration_vector[segment_idx], - extractor::guidance::TurnInstruction::NO_TURN(), - {{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID}, - travel_mode, - INVALID_ENTRY_CLASSID, - datasource_vector[segment_idx], - util::guidance::TurnBearing(0), - util::guidance::TurnBearing(0)}); - } - BOOST_ASSERT(unpacked_path.size() > 0); - if (facade.hasLaneData(turn_id)) - unpacked_path.back().lane_data = facade.GetLaneData(turn_id); - - unpacked_path.back().entry_classid = facade.GetEntryClassID(turn_id); - unpacked_path.back().turn_instruction = turn_instruction; - unpacked_path.back().duration_until_turn += facade.GetDurationPenaltyForEdgeID(turn_id); - unpacked_path.back().weight_until_turn += facade.GetWeightPenaltyForEdgeID(turn_id); - unpacked_path.back().pre_turn_bearing = facade.PreTurnBearing(turn_id); - unpacked_path.back().post_turn_bearing = facade.PostTurnBearing(turn_id); - }); std::size_t start_index = 0, end_index = 0; std::vector id_vector; @@ -536,140 +305,8 @@ void unpackPath(const FacadeT &facade, } } -/** - * Unpacks a single edge (NodeID->NodeID) from the CH graph down to it's original non-shortcut - * route. - * @param from the node the CH edge starts at - * @param to the node the CH edge finishes at - * @param unpacked_path the sequence of original NodeIDs that make up the expanded CH edge - */ -void unpackEdge(const datafacade::ContiguousInternalMemoryDataFacade &facade, - const NodeID from, - const NodeID to, - std::vector &unpacked_path); - -void retrievePackedPathFromHeap(const SearchEngineData::QueryHeap &forward_heap, - const SearchEngineData::QueryHeap &reverse_heap, - const NodeID middle_node_id, - std::vector &packed_path); - -void retrievePackedPathFromSingleHeap(const SearchEngineData::QueryHeap &search_heap, - const NodeID middle_node_id, - std::vector &packed_path); - -// assumes that heaps are already setup correctly. -// ATTENTION: This only works if no additional offset is supplied next to the Phantom Node -// Offsets. -// In case additional offsets are supplied, you might have to force a loop first. -// A forced loop might be necessary, if source and target are on the same segment. -// If this is the case and the offsets of the respective direction are larger for the source -// than the target -// then a force loop is required (e.g. source_phantom.forward_segment_id == -// target_phantom.forward_segment_id -// && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset()) -// requires -// a force loop, if the heaps have been initialized with positive offsets. -void search(const datafacade::ContiguousInternalMemoryDataFacade &facade, - SearchEngineData::QueryHeap &forward_heap, - SearchEngineData::QueryHeap &reverse_heap, - std::int32_t &weight, - std::vector &packed_leg, - const bool force_loop_forward, - const bool force_loop_reverse, - const int duration_upper_bound = INVALID_EDGE_WEIGHT); - -// Alias to be compatible with the overload for CoreCH that needs 4 heaps -inline void search(const datafacade::ContiguousInternalMemoryDataFacade &facade, - SearchEngineData::QueryHeap &forward_heap, - SearchEngineData::QueryHeap &reverse_heap, - SearchEngineData::QueryHeap &, - SearchEngineData::QueryHeap &, - std::int32_t &weight, - std::vector &packed_leg, - const bool force_loop_forward, - const bool force_loop_reverse, - const int duration_upper_bound = INVALID_EDGE_WEIGHT) -{ - search(facade, - forward_heap, - reverse_heap, - weight, - packed_leg, - force_loop_forward, - force_loop_reverse, - duration_upper_bound); -} - -// assumes that heaps are already setup correctly. -// A forced loop might be necessary, if source and target are on the same segment. -// If this is the case and the offsets of the respective direction are larger for the source -// than the target -// then a force loop is required (e.g. source_phantom.forward_segment_id == -// target_phantom.forward_segment_id -// && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset()) -// requires -// a force loop, if the heaps have been initialized with positive offsets. -void search(const datafacade::ContiguousInternalMemoryDataFacade &facade, - SearchEngineData::QueryHeap &forward_heap, - SearchEngineData::QueryHeap &reverse_heap, - SearchEngineData::QueryHeap &forward_core_heap, - SearchEngineData::QueryHeap &reverse_core_heap, - int &weight, - std::vector &packed_leg, - const bool force_loop_forward, - const bool force_loop_reverse, - int duration_upper_bound = INVALID_EDGE_WEIGHT); - -bool needsLoopForward(const PhantomNode &source_phantom, const PhantomNode &target_phantom); - -bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &target_phantom); - -double getPathDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, - const std::vector &packed_path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom); - -// Requires the heaps for be empty -// If heaps should be adjusted to be initialized outside of this function, -// the addition of force_loop parameters might be required -double -getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, - SearchEngineData::QueryHeap &forward_heap, - SearchEngineData::QueryHeap &reverse_heap, - SearchEngineData::QueryHeap &forward_core_heap, - SearchEngineData::QueryHeap &reverse_core_heap, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, - int duration_upper_bound = INVALID_EDGE_WEIGHT); - -// Requires the heaps for be empty -// If heaps should be adjusted to be initialized outside of this function, -// the addition of force_loop parameters might be required -double -getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, - SearchEngineData::QueryHeap &forward_heap, - SearchEngineData::QueryHeap &reverse_heap, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, - int duration_upper_bound = INVALID_EDGE_WEIGHT); - -// Alias to be compatible with the overload for CoreCH that needs 4 heaps -inline double -getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, - SearchEngineData::QueryHeap &forward_heap, - SearchEngineData::QueryHeap &reverse_heap, - SearchEngineData::QueryHeap &, - SearchEngineData::QueryHeap &, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, - int duration_upper_bound = INVALID_EDGE_WEIGHT) -{ - return getNetworkDistance( - facade, forward_heap, reverse_heap, source_phantom, target_phantom, duration_upper_bound); -} - } // namespace routing_algorithms } // namespace engine } // namespace osrm -#endif // ROUTING_BASE_HPP +#endif // OSRM_ENGINE_ROUTING_BASE_HPP diff --git a/include/engine/routing_algorithms/routing_base_ch.hpp b/include/engine/routing_algorithms/routing_base_ch.hpp new file mode 100644 index 00000000000..994e5e5c901 --- /dev/null +++ b/include/engine/routing_algorithms/routing_base_ch.hpp @@ -0,0 +1,475 @@ +#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 +{ + +namespace routing_algorithms +{ + +namespace ch +{ + +// Stalling +template +bool stallAtNode(const datafacade::ContiguousInternalMemoryDataFacade &facade, + const NodeID node, + const EdgeWeight weight, + const HeapT &query_heap) +{ + for (auto edge : facade.GetAdjacentEdgeRange(node)) + { + const auto &data = facade.GetEdgeData(edge); + if (DIRECTION == REVERSE_DIRECTION ? data.forward : data.backward) + { + const NodeID to = facade.GetTarget(edge); + const EdgeWeight edge_weight = data.weight; + BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid"); + if (query_heap.WasInserted(to)) + { + if (query_heap.GetKey(to) + edge_weight < weight) + { + return true; + } + } + } + } + return false; +} + +template +void relaxOutgoingEdges(const datafacade::ContiguousInternalMemoryDataFacade &facade, + const NodeID node, + const EdgeWeight weight, + SearchEngineData::QueryHeap &heap) +{ + for (const auto edge : facade.GetAdjacentEdgeRange(node)) + { + const auto &data = facade.GetEdgeData(edge); + if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward) + { + const NodeID to = facade.GetTarget(edge); + const EdgeWeight edge_weight = data.weight; + + BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid"); + const EdgeWeight to_weight = weight + edge_weight; + + // New Node discovered -> Add to Heap + Node Info Storage + if (!heap.WasInserted(to)) + { + heap.Insert(to, to_weight, node); + } + // Found a shorter Path -> Update weight + else if (to_weight < heap.GetKey(to)) + { + // new parent + heap.GetData(to).parent = node; + heap.DecreaseKey(to, to_weight); + } + } + } +} + +/* +min_edge_offset is needed in case we use multiple +nodes as start/target nodes with different (even negative) offsets. +In that case the termination criterion is not correct +anymore. + +Example: +forward heap: a(-100), b(0), +reverse heap: c(0), d(100) + +a --- d + \ / + / \ +b --- c + +This is equivalent to running a bi-directional Dijkstra on the following graph: + + a --- d + / \ / \ + y x z + \ / \ / + b --- c + +The graph is constructed by inserting nodes y and z that are connected to the initial nodes +using edges (y, a) with weight -100, (y, b) with weight 0 and, +(d, z) with weight 100, (c, z) with weight 0 corresponding. +Since we are dealing with a graph that contains _negative_ edges, +we need to add an offset to the termination criterion. +*/ +static constexpr bool ENABLE_STALLING = true; +static constexpr bool DISABLE_STALLING = false; +template +void routingStep(const datafacade::ContiguousInternalMemoryDataFacade &facade, + SearchEngineData::QueryHeap &forward_heap, + SearchEngineData::QueryHeap &reverse_heap, + NodeID &middle_node_id, + EdgeWeight &upper_bound, + EdgeWeight min_edge_offset, + const bool force_loop_forward, + const bool force_loop_reverse) +{ + const NodeID node = forward_heap.DeleteMin(); + const EdgeWeight weight = forward_heap.GetKey(node); + + if (reverse_heap.WasInserted(node)) + { + const EdgeWeight new_weight = reverse_heap.GetKey(node) + weight; + if (new_weight < upper_bound) + { + // if loops are forced, they are so at the source + if ((force_loop_forward && forward_heap.GetData(node).parent == node) || + (force_loop_reverse && reverse_heap.GetData(node).parent == node) || + // in this case we are looking at a bi-directional way where the source + // and target phantom are on the same edge based node + new_weight < 0) + { + // check whether there is a loop present at the node + for (const auto edge : facade.GetAdjacentEdgeRange(node)) + { + const auto &data = facade.GetEdgeData(edge); + if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward) + { + const NodeID to = facade.GetTarget(edge); + if (to == node) + { + const EdgeWeight edge_weight = data.weight; + const EdgeWeight loop_weight = new_weight + edge_weight; + if (loop_weight >= 0 && loop_weight < upper_bound) + { + middle_node_id = node; + upper_bound = loop_weight; + } + } + } + } + } + else + { + BOOST_ASSERT(new_weight >= 0); + + middle_node_id = node; + upper_bound = new_weight; + } + } + } + + // make sure we don't terminate too early if we initialize the weight + // for the nodes in the forward heap with the forward/reverse offset + BOOST_ASSERT(min_edge_offset <= 0); + if (weight + min_edge_offset > upper_bound) + { + forward_heap.DeleteAll(); + return; + } + + // Stalling + if (STALLING && stallAtNode(facade, node, weight, forward_heap)) + { + return; + } + + relaxOutgoingEdges(facade, node, weight, forward_heap); +} + +template +EdgeWeight +getLoopWeight(const datafacade::ContiguousInternalMemoryDataFacade &facade, + NodeID node) +{ + EdgeWeight loop_weight = UseDuration ? MAXIMAL_EDGE_DURATION : INVALID_EDGE_WEIGHT; + for (auto edge : facade.GetAdjacentEdgeRange(node)) + { + const auto &data = facade.GetEdgeData(edge); + if (data.forward) + { + const NodeID to = facade.GetTarget(edge); + if (to == node) + { + const auto value = UseDuration ? data.duration : data.weight; + loop_weight = std::min(loop_weight, value); + } + } + } + return loop_weight; +} + +/** + * Given a sequence of connected `NodeID`s in the CH graph, performs a depth-first unpacking of + * the shortcut + * edges. For every "original" edge found, it calls the `callback` with the two NodeIDs for the + * edge, and the EdgeData + * for that edge. + * + * The primary purpose of this unpacking is to expand a path through the CH into the original + * route through the + * pre-contracted graph. + * + * Because of the depth-first-search, the `callback` will effectively be called in sequence for + * the original route + * from beginning to end. + * + * @param packed_path_begin iterator pointing to the start of the NodeID list + * @param packed_path_end iterator pointing to the end of the NodeID list + * @param callback void(const std::pair, const EdgeID &) called for each + * original edge found. + */ +template +void unpackPath(const datafacade::ContiguousInternalMemoryDataFacade &facade, + BidirectionalIterator packed_path_begin, + BidirectionalIterator packed_path_end, + Callback &&callback) +{ + // make sure we have at least something to unpack + if (packed_path_begin == packed_path_end) + return; + + std::stack> recursion_stack; + + // We have to push the path in reverse order onto the stack because it's LIFO. + for (auto current = std::prev(packed_path_end); current != packed_path_begin; + current = std::prev(current)) + { + recursion_stack.emplace(*std::prev(current), *current); + } + + std::pair edge; + while (!recursion_stack.empty()) + { + edge = recursion_stack.top(); + recursion_stack.pop(); + + // Look for an edge on the forward CH graph (.forward) + EdgeID smaller_edge_id = facade.FindSmallestEdge( + edge.first, edge.second, [](const auto &data) { return data.forward; }); + + // If we didn't find one there, the we might be looking at a part of the path that + // was found using the backward search. Here, we flip the node order (.second, .first) + // and only consider edges with the `.backward` flag. + if (SPECIAL_EDGEID == smaller_edge_id) + { + smaller_edge_id = facade.FindSmallestEdge( + edge.second, edge.first, [](const auto &data) { return data.backward; }); + } + + // If we didn't find anything *still*, then something is broken and someone has + // called this function with bad values. + BOOST_ASSERT_MSG(smaller_edge_id != SPECIAL_EDGEID, "Invalid smaller edge ID"); + + const auto &data = facade.GetEdgeData(smaller_edge_id); + BOOST_ASSERT_MSG(data.weight != std::numeric_limits::max(), + "edge weight invalid"); + + // If the edge is a shortcut, we need to add the two halfs to the stack. + if (data.shortcut) + { // unpack + const NodeID middle_node_id = data.turn_id; + // Note the order here - we're adding these to a stack, so we + // want the first->middle to get visited before middle->second + recursion_stack.emplace(middle_node_id, edge.second); + recursion_stack.emplace(edge.first, middle_node_id); + } + else + { + // We found an original edge, call our callback. + std::forward(callback)(edge, smaller_edge_id); + } + } +} + +template +void unpackPath(const FacadeT &facade, + RandomIter packed_path_begin, + RandomIter packed_path_end, + const PhantomNodes &phantom_nodes, + std::vector &unpacked_path) +{ + const auto nodes_number = std::distance(packed_path_begin, packed_path_end); + BOOST_ASSERT(nodes_number > 0); + + std::vector unpacked_edges; + + auto source_node = *packed_path_begin, target_node = *packed_path_begin; + if (nodes_number > 1) + { + target_node = *std::prev(packed_path_end); + unpacked_edges.reserve(std::distance(packed_path_begin, packed_path_end)); + unpackPath( + facade, + packed_path_begin, + packed_path_end, + [&facade, &unpacked_edges](std::pair & /* edge */, + const auto &edge_id) { unpacked_edges.push_back(edge_id); }); + } + + annotatePath(facade, source_node, target_node, unpacked_edges, phantom_nodes, unpacked_path); +} + +/** + * Unpacks a single edge (NodeID->NodeID) from the CH graph down to it's original non-shortcut + * route. + * @param from the node the CH edge starts at + * @param to the node the CH edge finishes at + * @param unpacked_path the sequence of original NodeIDs that make up the expanded CH edge + */ +void unpackEdge(const datafacade::ContiguousInternalMemoryDataFacade &facade, + const NodeID from, + const NodeID to, + std::vector &unpacked_path); + +void retrievePackedPathFromHeap(const SearchEngineData::QueryHeap &forward_heap, + const SearchEngineData::QueryHeap &reverse_heap, + const NodeID middle_node_id, + std::vector &packed_path); + +void retrievePackedPathFromSingleHeap(const SearchEngineData::QueryHeap &search_heap, + const NodeID middle_node_id, + std::vector &packed_path); + +// assumes that heaps are already setup correctly. +// ATTENTION: This only works if no additional offset is supplied next to the Phantom Node +// Offsets. +// In case additional offsets are supplied, you might have to force a loop first. +// A forced loop might be necessary, if source and target are on the same segment. +// If this is the case and the offsets of the respective direction are larger for the source +// than the target +// then a force loop is required (e.g. source_phantom.forward_segment_id == +// target_phantom.forward_segment_id +// && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset()) +// requires +// a force loop, if the heaps have been initialized with positive offsets. +void search(const datafacade::ContiguousInternalMemoryDataFacade &facade, + SearchEngineData::QueryHeap &forward_heap, + SearchEngineData::QueryHeap &reverse_heap, + std::int32_t &weight, + std::vector &packed_leg, + const bool force_loop_forward, + const bool force_loop_reverse, + const int duration_upper_bound = INVALID_EDGE_WEIGHT); + +// Alias to be compatible with the overload for CoreCH that needs 4 heaps +inline void search(const datafacade::ContiguousInternalMemoryDataFacade &facade, + SearchEngineData::QueryHeap &forward_heap, + SearchEngineData::QueryHeap &reverse_heap, + SearchEngineData::QueryHeap &, + SearchEngineData::QueryHeap &, + std::int32_t &weight, + std::vector &packed_leg, + const bool force_loop_forward, + const bool force_loop_reverse, + const int duration_upper_bound = INVALID_EDGE_WEIGHT) +{ + search(facade, + forward_heap, + reverse_heap, + weight, + packed_leg, + force_loop_forward, + force_loop_reverse, + duration_upper_bound); +} + +// assumes that heaps are already setup correctly. +// A forced loop might be necessary, if source and target are on the same segment. +// If this is the case and the offsets of the respective direction are larger for the source +// than the target +// then a force loop is required (e.g. source_phantom.forward_segment_id == +// target_phantom.forward_segment_id +// && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset()) +// requires +// a force loop, if the heaps have been initialized with positive offsets. +void search(const datafacade::ContiguousInternalMemoryDataFacade &facade, + SearchEngineData::QueryHeap &forward_heap, + SearchEngineData::QueryHeap &reverse_heap, + SearchEngineData::QueryHeap &forward_core_heap, + SearchEngineData::QueryHeap &reverse_core_heap, + int &weight, + std::vector &packed_leg, + const bool force_loop_forward, + const bool force_loop_reverse, + int duration_upper_bound = INVALID_EDGE_WEIGHT); + +bool needsLoopForward(const PhantomNode &source_phantom, const PhantomNode &target_phantom); + +bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &target_phantom); + +double getPathDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, + const std::vector &packed_path, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom); + +// Requires the heaps for be empty +// If heaps should be adjusted to be initialized outside of this function, +// the addition of force_loop parameters might be required +double +getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, + SearchEngineData::QueryHeap &forward_heap, + SearchEngineData::QueryHeap &reverse_heap, + SearchEngineData::QueryHeap &forward_core_heap, + SearchEngineData::QueryHeap &reverse_core_heap, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + int duration_upper_bound = INVALID_EDGE_WEIGHT); + +// Requires the heaps for be empty +// If heaps should be adjusted to be initialized outside of this function, +// the addition of force_loop parameters might be required +double +getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, + SearchEngineData::QueryHeap &forward_heap, + SearchEngineData::QueryHeap &reverse_heap, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + int duration_upper_bound = INVALID_EDGE_WEIGHT); + +// Alias to be compatible with the overload for CoreCH that needs 4 heaps +inline double +getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, + SearchEngineData::QueryHeap &forward_heap, + SearchEngineData::QueryHeap &reverse_heap, + SearchEngineData::QueryHeap &, + SearchEngineData::QueryHeap &, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + int duration_upper_bound = INVALID_EDGE_WEIGHT) +{ + return getNetworkDistance( + facade, forward_heap, reverse_heap, source_phantom, target_phantom, duration_upper_bound); +} + +} // namespace ch +} // namespace routing_algorithms +} // namespace engine +} // namespace osrm + +#endif // OSRM_ENGINE_ROUTING_BASE_CH_HPP diff --git a/src/engine/routing_algorithms/alternative_path.cpp b/src/engine/routing_algorithms/alternative_path.cpp index a75e0772c4c..bd75721907e 100644 --- a/src/engine/routing_algorithms/alternative_path.cpp +++ b/src/engine/routing_algorithms/alternative_path.cpp @@ -1,5 +1,5 @@ #include "engine/routing_algorithms/alternative_path.hpp" -#include "engine/routing_algorithms/routing_base.hpp" +#include "engine/routing_algorithms/routing_base_ch.hpp" #include "util/integer_range.hpp" @@ -89,7 +89,7 @@ void alternativeRoutingStep( else { // check whether there is a loop present at the node - const auto loop_weight = getLoopWeight(facade, node); + const auto loop_weight = ch::getLoopWeight(facade, node); const EdgeWeight new_weight_with_loop = new_weight + loop_weight; if (loop_weight != INVALID_EDGE_WEIGHT && new_weight_with_loop <= *upper_bound_to_shortest_path_weight) @@ -139,11 +139,11 @@ void retrievePackedAlternatePath(const QueryHeap &forward_heap1, { // fetch packed path [s,v) std::vector packed_v_t_path; - retrievePackedPathFromHeap(forward_heap1, reverse_heap2, s_v_middle, packed_path); + ch::retrievePackedPathFromHeap(forward_heap1, reverse_heap2, s_v_middle, packed_path); packed_path.pop_back(); // remove middle node. It's in both half-paths // fetch patched path [v,t] - retrievePackedPathFromHeap(forward_heap2, reverse_heap1, v_t_middle, packed_v_t_path); + ch::retrievePackedPathFromHeap(forward_heap2, reverse_heap1, v_t_middle, packed_v_t_path); packed_path.insert(packed_path.end(), packed_v_t_path.begin(), packed_v_t_path.end()); } @@ -180,14 +180,14 @@ void computeLengthAndSharingOfViaPath( // compute path by reusing forward search from s while (!new_reverse_heap.Empty()) { - routingStep(facade, - new_reverse_heap, - existing_forward_heap, - s_v_middle, - upper_bound_s_v_path_length, - min_edge_offset, - DO_NOT_FORCE_LOOPS, - DO_NOT_FORCE_LOOPS); + ch::routingStep(facade, + new_reverse_heap, + existing_forward_heap, + s_v_middle, + upper_bound_s_v_path_length, + min_edge_offset, + DO_NOT_FORCE_LOOPS, + DO_NOT_FORCE_LOOPS); } // compute path by reusing backward search from node t NodeID v_t_middle = SPECIAL_NODEID; @@ -195,14 +195,14 @@ void computeLengthAndSharingOfViaPath( new_forward_heap.Insert(via_node, 0, via_node); while (!new_forward_heap.Empty()) { - routingStep(facade, - new_forward_heap, - existing_reverse_heap, - v_t_middle, - upper_bound_of_v_t_path_length, - min_edge_offset, - DO_NOT_FORCE_LOOPS, - DO_NOT_FORCE_LOOPS); + ch::routingStep(facade, + new_forward_heap, + existing_reverse_heap, + v_t_middle, + upper_bound_of_v_t_path_length, + min_edge_offset, + DO_NOT_FORCE_LOOPS, + DO_NOT_FORCE_LOOPS); } *real_length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length; @@ -212,9 +212,9 @@ void computeLengthAndSharingOfViaPath( } // retrieve packed paths - retrievePackedPathFromHeap( + ch::retrievePackedPathFromHeap( existing_forward_heap, new_reverse_heap, s_v_middle, packed_s_v_path); - retrievePackedPathFromHeap( + ch::retrievePackedPathFromHeap( new_forward_heap, existing_reverse_heap, v_t_middle, packed_v_t_path); // partial unpacking, compute sharing @@ -234,14 +234,14 @@ void computeLengthAndSharingOfViaPath( { if (packed_s_v_path[current_node] == packed_shortest_path[current_node]) { - unpackEdge(facade, - packed_s_v_path[current_node], - packed_s_v_path[current_node + 1], - partially_unpacked_via_path); - unpackEdge(facade, - packed_shortest_path[current_node], - packed_shortest_path[current_node + 1], - partially_unpacked_shortest_path); + ch::unpackEdge(facade, + packed_s_v_path[current_node], + packed_s_v_path[current_node + 1], + partially_unpacked_via_path); + ch::unpackEdge(facade, + packed_shortest_path[current_node], + packed_shortest_path[current_node + 1], + partially_unpacked_shortest_path); break; } } @@ -280,14 +280,14 @@ void computeLengthAndSharingOfViaPath( { if (packed_v_t_path[via_path_index] == packed_shortest_path[shortest_path_index]) { - unpackEdge(facade, - packed_v_t_path[via_path_index - 1], - packed_v_t_path[via_path_index], - partially_unpacked_via_path); - unpackEdge(facade, - packed_shortest_path[shortest_path_index - 1], - packed_shortest_path[shortest_path_index], - partially_unpacked_shortest_path); + ch::unpackEdge(facade, + packed_v_t_path[via_path_index - 1], + packed_v_t_path[via_path_index], + partially_unpacked_via_path); + ch::unpackEdge(facade, + packed_shortest_path[shortest_path_index - 1], + packed_shortest_path[shortest_path_index], + partially_unpacked_shortest_path); break; } } @@ -342,14 +342,14 @@ bool viaNodeCandidatePassesTTest( new_reverse_heap.Insert(candidate.node, 0, candidate.node); while (new_reverse_heap.Size() > 0) { - routingStep(facade, - new_reverse_heap, - existing_forward_heap, - *s_v_middle, - upper_bound_s_v_path_length, - min_edge_offset, - DO_NOT_FORCE_LOOPS, - DO_NOT_FORCE_LOOPS); + ch::routingStep(facade, + new_reverse_heap, + existing_forward_heap, + *s_v_middle, + upper_bound_s_v_path_length, + min_edge_offset, + DO_NOT_FORCE_LOOPS, + DO_NOT_FORCE_LOOPS); } if (INVALID_EDGE_WEIGHT == upper_bound_s_v_path_length) @@ -363,14 +363,14 @@ bool viaNodeCandidatePassesTTest( new_forward_heap.Insert(candidate.node, 0, candidate.node); while (new_forward_heap.Size() > 0) { - routingStep(facade, - new_forward_heap, - existing_reverse_heap, - *v_t_middle, - upper_bound_of_v_t_path_length, - min_edge_offset, - DO_NOT_FORCE_LOOPS, - DO_NOT_FORCE_LOOPS); + ch::routingStep(facade, + new_forward_heap, + existing_reverse_heap, + *v_t_middle, + upper_bound_of_v_t_path_length, + min_edge_offset, + DO_NOT_FORCE_LOOPS, + DO_NOT_FORCE_LOOPS); } if (INVALID_EDGE_WEIGHT == upper_bound_of_v_t_path_length) @@ -381,10 +381,10 @@ bool viaNodeCandidatePassesTTest( *length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length; // retrieve packed paths - retrievePackedPathFromHeap( + ch::retrievePackedPathFromHeap( existing_forward_heap, new_reverse_heap, *s_v_middle, packed_s_v_path); - retrievePackedPathFromHeap( + ch::retrievePackedPathFromHeap( new_forward_heap, existing_reverse_heap, *v_t_middle, packed_v_t_path); NodeID s_P = *s_v_middle, t_P = *v_t_middle; @@ -536,25 +536,25 @@ bool viaNodeCandidatePassesTTest( { if (!forward_heap3.Empty()) { - routingStep(facade, - forward_heap3, - reverse_heap3, - middle, - upper_bound, - min_edge_offset, - DO_NOT_FORCE_LOOPS, - DO_NOT_FORCE_LOOPS); + ch::routingStep(facade, + forward_heap3, + reverse_heap3, + middle, + upper_bound, + min_edge_offset, + DO_NOT_FORCE_LOOPS, + DO_NOT_FORCE_LOOPS); } if (!reverse_heap3.Empty()) { - routingStep(facade, - reverse_heap3, - forward_heap3, - middle, - upper_bound, - min_edge_offset, - DO_NOT_FORCE_LOOPS, - DO_NOT_FORCE_LOOPS); + ch::routingStep(facade, + reverse_heap3, + forward_heap3, + middle, + upper_bound, + min_edge_offset, + DO_NOT_FORCE_LOOPS, + DO_NOT_FORCE_LOOPS); } } return (upper_bound <= t_test_path_length); @@ -593,35 +593,7 @@ alternativePathSearch(SearchEngineData &engine_working_data, ? -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset() : 0); - if (phantom_node_pair.source_phantom.forward_segment_id.enabled) - { - BOOST_ASSERT(phantom_node_pair.source_phantom.forward_segment_id.id != SPECIAL_SEGMENTID); - forward_heap1.Insert(phantom_node_pair.source_phantom.forward_segment_id.id, - -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(), - phantom_node_pair.source_phantom.forward_segment_id.id); - } - if (phantom_node_pair.source_phantom.reverse_segment_id.enabled) - { - BOOST_ASSERT(phantom_node_pair.source_phantom.reverse_segment_id.id != SPECIAL_SEGMENTID); - forward_heap1.Insert(phantom_node_pair.source_phantom.reverse_segment_id.id, - -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(), - phantom_node_pair.source_phantom.reverse_segment_id.id); - } - - if (phantom_node_pair.target_phantom.forward_segment_id.enabled) - { - BOOST_ASSERT(phantom_node_pair.target_phantom.forward_segment_id.id != SPECIAL_SEGMENTID); - reverse_heap1.Insert(phantom_node_pair.target_phantom.forward_segment_id.id, - phantom_node_pair.target_phantom.GetForwardWeightPlusOffset(), - phantom_node_pair.target_phantom.forward_segment_id.id); - } - if (phantom_node_pair.target_phantom.reverse_segment_id.enabled) - { - BOOST_ASSERT(phantom_node_pair.target_phantom.reverse_segment_id.id != SPECIAL_SEGMENTID); - reverse_heap1.Insert(phantom_node_pair.target_phantom.reverse_segment_id.id, - phantom_node_pair.target_phantom.GetReverseWeightPlusOffset(), - phantom_node_pair.target_phantom.reverse_segment_id.id); - } + insertNodesInHeaps(forward_heap1, reverse_heap1, phantom_node_pair); // search from s and t till new_min/(1+epsilon) > length_of_shortest_path while (0 < (forward_heap1.Size() + reverse_heap1.Size())) @@ -674,8 +646,8 @@ alternativePathSearch(SearchEngineData &engine_working_data, else { - retrievePackedPathFromSingleHeap(forward_heap1, middle_node, packed_forward_path); - retrievePackedPathFromSingleHeap(reverse_heap1, middle_node, packed_reverse_path); + ch::retrievePackedPathFromSingleHeap(forward_heap1, middle_node, packed_forward_path); + ch::retrievePackedPathFromSingleHeap(reverse_heap1, middle_node, packed_reverse_path); } // this set is is used as an indicator if a node is on the shortest path @@ -827,14 +799,14 @@ alternativePathSearch(SearchEngineData &engine_working_data, raw_route_data.target_traversed_in_reverse.push_back(( packed_shortest_path.back() != phantom_node_pair.target_phantom.forward_segment_id.id)); - unpackPath(facade, - // -- packed input - packed_shortest_path.begin(), - packed_shortest_path.end(), - // -- start of route - phantom_node_pair, - // -- unpacked output - raw_route_data.unpacked_path_segments.front()); + ch::unpackPath(facade, + // -- packed input + packed_shortest_path.begin(), + packed_shortest_path.end(), + // -- start of route + phantom_node_pair, + // -- unpacked output + raw_route_data.unpacked_path_segments.front()); raw_route_data.shortest_path_length = upper_bound_to_shortest_path_weight; } @@ -858,11 +830,11 @@ alternativePathSearch(SearchEngineData &engine_working_data, phantom_node_pair.target_phantom.forward_segment_id.id)); // unpack the alternate path - unpackPath(facade, - packed_alternate_path.begin(), - packed_alternate_path.end(), - phantom_node_pair, - raw_route_data.unpacked_alternative); + ch::unpackPath(facade, + packed_alternate_path.begin(), + packed_alternate_path.end(), + phantom_node_pair, + raw_route_data.unpacked_alternative); raw_route_data.alternative_path_length = length_of_via_path; } diff --git a/src/engine/routing_algorithms/direct_shortest_path.cpp b/src/engine/routing_algorithms/direct_shortest_path.cpp index 93465b8b9c9..efcbd593117 100644 --- a/src/engine/routing_algorithms/direct_shortest_path.cpp +++ b/src/engine/routing_algorithms/direct_shortest_path.cpp @@ -1,6 +1,7 @@ #include "engine/routing_algorithms/direct_shortest_path.hpp" #include "engine/routing_algorithms/routing_base.hpp" +#include "engine/routing_algorithms/routing_base_ch.hpp" namespace osrm { @@ -9,44 +10,8 @@ namespace engine namespace routing_algorithms { -namespace +namespace ch { -void insertInHeaps(SearchEngineData::QueryHeap &forward_heap, - SearchEngineData::QueryHeap &reverse_heap, - const PhantomNodes &nodes) -{ - const auto &source_phantom = nodes.source_phantom; - const auto &target_phantom = nodes.target_phantom; - BOOST_ASSERT(source_phantom.IsValid()); - BOOST_ASSERT(target_phantom.IsValid()); - - if (source_phantom.forward_segment_id.enabled) - { - forward_heap.Insert(source_phantom.forward_segment_id.id, - -source_phantom.GetForwardWeightPlusOffset(), - source_phantom.forward_segment_id.id); - } - if (source_phantom.reverse_segment_id.enabled) - { - forward_heap.Insert(source_phantom.reverse_segment_id.id, - -source_phantom.GetReverseWeightPlusOffset(), - source_phantom.reverse_segment_id.id); - } - - if (target_phantom.forward_segment_id.enabled) - { - reverse_heap.Insert(target_phantom.forward_segment_id.id, - target_phantom.GetForwardWeightPlusOffset(), - target_phantom.forward_segment_id.id); - } - - if (target_phantom.reverse_segment_id.enabled) - { - reverse_heap.Insert(target_phantom.reverse_segment_id.id, - target_phantom.GetReverseWeightPlusOffset(), - target_phantom.reverse_segment_id.id); - } -} template InternalRouteResult @@ -82,7 +47,6 @@ extractRoute(const datafacade::ContiguousInternalMemoryDataFacade &f return raw_route_data; } -} /// 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 @@ -109,7 +73,7 @@ InternalRouteResult directShortestPathSearchImpl( int weight = INVALID_EDGE_WEIGHT; std::vector packed_leg; - insertInHeaps(forward_heap, reverse_heap, phantom_nodes); + insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes); search(facade, forward_heap, @@ -124,12 +88,14 @@ InternalRouteResult directShortestPathSearchImpl( return extractRoute(facade, weight, packed_leg, phantom_nodes); } +} // namespace ch + InternalRouteResult directShortestPathSearch( SearchEngineData &engine_working_data, const datafacade::ContiguousInternalMemoryDataFacade &facade, const PhantomNodes &phantom_nodes) { - return directShortestPathSearchImpl(engine_working_data, facade, phantom_nodes); + return ch::directShortestPathSearchImpl(engine_working_data, facade, phantom_nodes); } InternalRouteResult directShortestPathSearch( @@ -137,7 +103,7 @@ InternalRouteResult directShortestPathSearch( const datafacade::ContiguousInternalMemoryDataFacade &facade, const PhantomNodes &phantom_nodes) { - return directShortestPathSearchImpl(engine_working_data, facade, phantom_nodes); + return ch::directShortestPathSearchImpl(engine_working_data, facade, phantom_nodes); } } // namespace routing_algorithms diff --git a/src/engine/routing_algorithms/many_to_many.cpp b/src/engine/routing_algorithms/many_to_many.cpp index 6eef3b0d1db..64e30e9cbbc 100644 --- a/src/engine/routing_algorithms/many_to_many.cpp +++ b/src/engine/routing_algorithms/many_to_many.cpp @@ -1,5 +1,5 @@ #include "engine/routing_algorithms/many_to_many.hpp" -#include "engine/routing_algorithms/routing_base.hpp" +#include "engine/routing_algorithms/routing_base_ch.hpp" #include @@ -101,14 +101,14 @@ void forwardRoutingStep(const datafacade::ContiguousInternalMemoryDataFacade(facade, node); + const EdgeWeight loop_weight = ch::getLoopWeight(facade, node); const EdgeWeight new_weight_with_loop = new_weight + loop_weight; if (loop_weight != INVALID_EDGE_WEIGHT && new_weight_with_loop >= 0) { current_weight = std::min(current_weight, new_weight_with_loop); current_duration = std::min(current_duration, source_duration + target_duration + - getLoopWeight(facade, node)); + ch::getLoopWeight(facade, node)); } } else if (new_weight < current_weight) @@ -118,7 +118,7 @@ void forwardRoutingStep(const datafacade::ContiguousInternalMemoryDataFacade(facade, node, source_weight, query_heap)) + if (ch::stallAtNode(facade, node, source_weight, query_heap)) { return; } @@ -139,7 +139,7 @@ void backwardRoutingStep( // store settled nodes in search space bucket search_space_with_buckets[node].emplace_back(column_idx, target_weight, target_duration); - if (stallAtNode(facade, node, target_weight, query_heap)) + if (ch::stallAtNode(facade, node, target_weight, query_heap)) { return; } @@ -172,21 +172,9 @@ manyToManySearch(SearchEngineData &engine_working_data, unsigned column_idx = 0; const auto search_target_phantom = [&](const PhantomNode &phantom) { + // clear heap and insert target nodes query_heap.Clear(); - // insert target(s) at weight 0 - - if (phantom.forward_segment_id.enabled) - { - query_heap.Insert(phantom.forward_segment_id.id, - phantom.GetForwardWeightPlusOffset(), - {phantom.forward_segment_id.id, phantom.GetForwardDuration()}); - } - if (phantom.reverse_segment_id.enabled) - { - query_heap.Insert(phantom.reverse_segment_id.id, - phantom.GetReverseWeightPlusOffset(), - {phantom.reverse_segment_id.id, phantom.GetReverseDuration()}); - } + insertNodesInHeap(query_heap, phantom); // explore search space while (!query_heap.Empty()) @@ -199,21 +187,9 @@ manyToManySearch(SearchEngineData &engine_working_data, // for each source do forward search unsigned row_idx = 0; const auto search_source_phantom = [&](const PhantomNode &phantom) { + // clear heap and insert source nodes query_heap.Clear(); - // insert target(s) at weight 0 - - if (phantom.forward_segment_id.enabled) - { - query_heap.Insert(phantom.forward_segment_id.id, - -phantom.GetForwardWeightPlusOffset(), - {phantom.forward_segment_id.id, -phantom.GetForwardDuration()}); - } - if (phantom.reverse_segment_id.enabled) - { - query_heap.Insert(phantom.reverse_segment_id.id, - -phantom.GetReverseWeightPlusOffset(), - {phantom.reverse_segment_id.id, -phantom.GetReverseDuration()}); - } + insertNodesInHeap(query_heap, phantom); // explore search space while (!query_heap.Empty()) diff --git a/src/engine/routing_algorithms/map_matching.cpp b/src/engine/routing_algorithms/map_matching.cpp index 3975322ccd5..e5cbb54690c 100644 --- a/src/engine/routing_algorithms/map_matching.cpp +++ b/src/engine/routing_algorithms/map_matching.cpp @@ -1,5 +1,5 @@ #include "engine/routing_algorithms/map_matching.hpp" -#include "engine/routing_algorithms/routing_base.hpp" +#include "engine/routing_algorithms/routing_base_ch.hpp" #include "engine/map_matching/hidden_markov_model.hpp" #include "engine/map_matching/matching_confidence.hpp" @@ -210,14 +210,14 @@ mapMatchingImpl(SearchEngineData &engine_working_data, } double network_distance = - getNetworkDistance(facade, - forward_heap, - reverse_heap, - forward_core_heap, - reverse_core_heap, - prev_unbroken_timestamps_list[s].phantom_node, - current_timestamps_list[s_prime].phantom_node, - duration_upper_bound); + ch::getNetworkDistance(facade, + forward_heap, + reverse_heap, + forward_core_heap, + reverse_core_heap, + prev_unbroken_timestamps_list[s].phantom_node, + current_timestamps_list[s_prime].phantom_node, + duration_upper_bound); // get distance diff between loc1/2 and locs/s_prime const auto d_t = std::abs(network_distance - haversine_distance); diff --git a/src/engine/routing_algorithms/routing_base.cpp b/src/engine/routing_algorithms/routing_base_ch.cpp similarity index 94% rename from src/engine/routing_algorithms/routing_base.cpp rename to src/engine/routing_algorithms/routing_base_ch.cpp index 2d56e74b9d1..484e3b8e353 100644 --- a/src/engine/routing_algorithms/routing_base.cpp +++ b/src/engine/routing_algorithms/routing_base_ch.cpp @@ -1,4 +1,4 @@ -#include "engine/routing_algorithms/routing_base.hpp" +#include "engine/routing_algorithms/routing_base_ch.hpp" namespace osrm { @@ -6,6 +6,8 @@ namespace engine { namespace routing_algorithms { +namespace ch +{ /** * Unpacks a single edge (NodeID->NodeID) from the CH graph down to it's original non-shortcut @@ -411,31 +413,7 @@ getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade packed_path; @@ -517,6 +495,7 @@ getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade #include @@ -71,23 +71,24 @@ void searchWithUTurn(const datafacade::ContiguousInternalMemoryDataFacade &fa reverse_core_heap.Clear(); BOOST_ASSERT(forward_core_heap.Size() == 0); BOOST_ASSERT(reverse_core_heap.Size() == 0); - routing_algorithms::search(facade, - forward_heap, - reverse_heap, - forward_core_heap, - reverse_core_heap, - new_total_weight_to_forward, - leg_packed_path_forward, - needsLoopForward(source_phantom, target_phantom), - DO_NOT_FORCE_LOOP); + ch::search(facade, + forward_heap, + reverse_heap, + forward_core_heap, + reverse_core_heap, + new_total_weight_to_forward, + leg_packed_path_forward, + ch::needsLoopForward(source_phantom, target_phantom), + DO_NOT_FORCE_LOOP); } if (search_to_reverse_node) @@ -185,15 +186,15 @@ void search(const datafacade::ContiguousInternalMemoryDataFacade &fa reverse_core_heap.Clear(); BOOST_ASSERT(forward_core_heap.Size() == 0); BOOST_ASSERT(reverse_core_heap.Size() == 0); - routing_algorithms::search(facade, - forward_heap, - reverse_heap, - forward_core_heap, - reverse_core_heap, - new_total_weight_to_reverse, - leg_packed_path_reverse, - DO_NOT_FORCE_LOOP, - needsLoopBackwards(source_phantom, target_phantom)); + ch::search(facade, + forward_heap, + reverse_heap, + forward_core_heap, + reverse_core_heap, + new_total_weight_to_reverse, + leg_packed_path_reverse, + DO_NOT_FORCE_LOOP, + ch::needsLoopBackwards(source_phantom, target_phantom)); } } @@ -213,11 +214,11 @@ void unpackLegs(const datafacade::ContiguousInternalMemoryDataFacade