diff --git a/plugins/viaroute.hpp b/plugins/viaroute.hpp index 293590d66..40b7ef63f 100644 --- a/plugins/viaroute.hpp +++ b/plugins/viaroute.hpp @@ -114,6 +114,12 @@ template class ViaRoutePlugin final : public BasePlugin { phantom_node_pair_list[i].second = phantom_node_vector.back(); } + + } + else + { + json_result.values["status_message"] = std::string("Could not find a matching segment for coordinate ") + std::to_string(i); + return 400; } } @@ -172,8 +178,8 @@ template class ViaRoutePlugin final : public BasePlugin { if (route_parameters.alternate_route) { - search_engine_ptr->alternative_path(raw_route.segment_end_coordinates.front(), - raw_route); + search_engine_ptr->alternative_path(raw_route.segment_end_coordinates.front(), + raw_route); } else { diff --git a/routing_algorithms/alternative_path.hpp b/routing_algorithms/alternative_path.hpp index 5e56147f6..59b772edb 100644 --- a/routing_algorithms/alternative_path.hpp +++ b/routing_algorithms/alternative_path.hpp @@ -317,7 +317,7 @@ class AlternativeRouting final super::UnpackPath( // -- packed input - packed_shortest_path, + packed_shortest_path.begin(), packed_shortest_path.end(), // -- start of route phantom_node_pair, // -- unpacked output @@ -338,8 +338,8 @@ class AlternativeRouting final (packed_alternate_path.back() != phantom_node_pair.target_phantom.forward_node_id)); // unpack the alternate path - super::UnpackPath(packed_alternate_path, phantom_node_pair, - raw_route_data.unpacked_alternative); + super::UnpackPath(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; } @@ -401,8 +401,8 @@ class AlternativeRouting final // compute path by reusing forward search from s while (!new_reverse_heap.Empty()) { - super::RoutingStep(new_reverse_heap, existing_forward_heap, &s_v_middle, - &upper_bound_s_v_path_length, min_edge_offset, false); + super::RoutingStep(new_reverse_heap, existing_forward_heap, s_v_middle, + upper_bound_s_v_path_length, min_edge_offset, false); } // compute path by reusing backward search from node t NodeID v_t_middle = SPECIAL_NODEID; @@ -410,8 +410,8 @@ class AlternativeRouting final new_forward_heap.Insert(via_node, 0, via_node); while (!new_forward_heap.Empty()) { - super::RoutingStep(new_forward_heap, existing_reverse_heap, &v_t_middle, - &upper_bound_of_v_t_path_length, min_edge_offset, true); + super::RoutingStep(new_forward_heap, existing_reverse_heap, v_t_middle, + upper_bound_of_v_t_path_length, min_edge_offset, true); } *real_length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length; @@ -459,10 +459,10 @@ class AlternativeRouting final partially_unpacked_shortest_path.size())) - 1; for (int64_t current_node = 0; (current_node < packed_path_length) && - (partially_unpacked_via_path[current_node] == - partially_unpacked_shortest_path[current_node] && - partially_unpacked_via_path[current_node + 1] == - partially_unpacked_shortest_path[current_node + 1]); + (partially_unpacked_via_path[current_node] == + partially_unpacked_shortest_path[current_node] && + partially_unpacked_via_path[current_node + 1] == + partially_unpacked_shortest_path[current_node + 1]); ++current_node) { EdgeID selected_edge = @@ -671,8 +671,8 @@ class AlternativeRouting final new_reverse_heap.Insert(candidate.node, 0, candidate.node); while (new_reverse_heap.Size() > 0) { - super::RoutingStep(new_reverse_heap, existing_forward_heap, s_v_middle, - &upper_bound_s_v_path_length, min_edge_offset, false); + super::RoutingStep(new_reverse_heap, existing_forward_heap, *s_v_middle, + upper_bound_s_v_path_length, min_edge_offset, false); } if (INVALID_EDGE_WEIGHT == upper_bound_s_v_path_length) @@ -686,8 +686,8 @@ class AlternativeRouting final new_forward_heap.Insert(candidate.node, 0, candidate.node); while (new_forward_heap.Size() > 0) { - super::RoutingStep(new_forward_heap, existing_reverse_heap, v_t_middle, - &upper_bound_of_v_t_path_length, min_edge_offset, true); + super::RoutingStep(new_forward_heap, existing_reverse_heap, *v_t_middle, + upper_bound_of_v_t_path_length, min_edge_offset, true); } if (INVALID_EDGE_WEIGHT == upper_bound_of_v_t_path_length) @@ -854,12 +854,12 @@ class AlternativeRouting final { if (!forward_heap3.Empty()) { - super::RoutingStep(forward_heap3, reverse_heap3, &middle, &upper_bound, + super::RoutingStep(forward_heap3, reverse_heap3, middle, upper_bound, min_edge_offset, true); } if (!reverse_heap3.Empty()) { - super::RoutingStep(reverse_heap3, forward_heap3, &middle, &upper_bound, + super::RoutingStep(reverse_heap3, forward_heap3, middle, upper_bound, min_edge_offset, false); } } diff --git a/routing_algorithms/direct_shortest_path.hpp b/routing_algorithms/direct_shortest_path.hpp index 09e39c73e..2237d682a 100644 --- a/routing_algorithms/direct_shortest_path.hpp +++ b/routing_algorithms/direct_shortest_path.hpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define DIRECT_SHORTEST_PATH_HPP #include +#include #include "routing_base.hpp" #include "../data_structures/search_engine_data.hpp" @@ -68,6 +69,8 @@ class DirectShortestPathRouting final BOOST_ASSERT_MSG(1 == phantom_nodes_vector.size(), "Direct Shortest Path Query only accepts a single source and target pair. Multiple ones have been specified."); const auto& phantom_node_pair = phantom_nodes_vector.front(); + const auto& source_phantom = phantom_node_pair.source_phantom; + const auto& target_phantom = phantom_node_pair.target_phantom; engine_working_data.InitializeOrClearFirstThreadLocalStorage( super::facade->GetNumberOfNodes()); @@ -76,7 +79,37 @@ class DirectShortestPathRouting final forward_heap.Clear(); reverse_heap.Clear(); - int distance; + BOOST_ASSERT(source_phantom.is_valid()); + BOOST_ASSERT(target_phantom.is_valid()); + + if (source_phantom.forward_node_id != SPECIAL_NODEID) + { + forward_heap.Insert(source_phantom.forward_node_id, + -source_phantom.GetForwardWeightPlusOffset(), + source_phantom.forward_node_id); + } + if (source_phantom.reverse_node_id != SPECIAL_NODEID) + { + forward_heap.Insert(source_phantom.reverse_node_id, + -source_phantom.GetReverseWeightPlusOffset(), + source_phantom.reverse_node_id); + } + + if (target_phantom.forward_node_id != SPECIAL_NODEID) + { + reverse_heap.Insert(target_phantom.forward_node_id, + target_phantom.GetForwardWeightPlusOffset(), + target_phantom.forward_node_id); + } + + if (target_phantom.reverse_node_id != SPECIAL_NODEID) + { + reverse_heap.Insert(target_phantom.reverse_node_id, + target_phantom.GetReverseWeightPlusOffset(), + target_phantom.reverse_node_id); + } + + int distance = INVALID_EDGE_WEIGHT; std::vector packed_leg; if (super::facade->GetCoreSize() > 0) @@ -90,13 +123,11 @@ class DirectShortestPathRouting final super::SearchWithCore(forward_heap, reverse_heap, forward_core_heap, reverse_core_heap, - phantom_node_pair.source_phantom, phantom_node_pair.target_phantom, distance, packed_leg); } else { - super::Search(forward_heap, reverse_heap, phantom_node_pair.source_phantom, - phantom_node_pair.target_phantom, distance, packed_leg); + super::Search(forward_heap, reverse_heap, distance, packed_leg); } // No path found for both target nodes? @@ -116,7 +147,7 @@ class DirectShortestPathRouting final raw_route_data.target_traversed_in_reverse.push_back( (packed_leg.back() != phantom_node_pair.target_phantom.forward_node_id)); - super::UnpackPath(packed_leg, phantom_node_pair, raw_route_data.unpacked_path_segments.front()); + super::UnpackPath(packed_leg.begin(), packed_leg.end(), phantom_node_pair, raw_route_data.unpacked_path_segments.front()); } }; diff --git a/routing_algorithms/routing_base.hpp b/routing_algorithms/routing_base.hpp index 360950d20..4ec491cf5 100644 --- a/routing_algorithms/routing_base.hpp +++ b/routing_algorithms/routing_base.hpp @@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "../data_structures/internal_route_result.hpp" #include "../data_structures/search_engine_data.hpp" #include "../data_structures/turn_instructions.hpp" -// #include "../util/simple_logger.hpp" #include @@ -59,42 +58,62 @@ template class BasicRoutingInterface explicit BasicRoutingInterface(DataFacadeT *facade) : facade(facade) {} ~BasicRoutingInterface() {} + // 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. void RoutingStep(SearchEngineData::QueryHeap &forward_heap, SearchEngineData::QueryHeap &reverse_heap, - NodeID *middle_node_id, - int *upper_bound, - const int min_edge_offset, + NodeID &middle_node_id, + int &upper_bound, + int min_edge_offset, const bool forward_direction, - const bool stalling=true) const + const bool stalling = true) const { const NodeID node = forward_heap.DeleteMin(); const int distance = forward_heap.GetKey(node); - // const NodeID parentnode = forward_heap.GetData(node).parent; - // SimpleLogger().Write() << (forward_direction ? "[fwd] " : "[rev] ") << "settled edge (" - // << parentnode << "," << node << "), dist: " << distance; - if (reverse_heap.WasInserted(node)) { const int new_distance = reverse_heap.GetKey(node) + distance; - if (new_distance < *upper_bound) + if (new_distance < upper_bound) { if (new_distance >= 0) { - *middle_node_id = node; - *upper_bound = new_distance; - // SimpleLogger().Write() << "accepted middle node " << node << " at - // distance " << new_distance; - // } else { - // SimpleLogger().Write() << "discared middle node " << node << " at - // distance " << new_distance; + middle_node_id = node; + upper_bound = new_distance; } } } - if (distance + min_edge_offset > *upper_bound) + // make sure we don't terminate too early if we initialize the distance + // for the nodes in the forward heap with the forward/reverse offset + BOOST_ASSERT(min_edge_offset <= 0); + if (distance + min_edge_offset > upper_bound) { - // SimpleLogger().Write() << "min_edge_offset: " << min_edge_offset; forward_heap.DeleteAll(); return; } @@ -102,26 +121,26 @@ template class BasicRoutingInterface // Stalling if (stalling) { - for (const auto edge : facade->GetAdjacentEdgeRange(node)) - { - const EdgeData &data = facade->GetEdgeData(edge); - const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward); - if (reverse_flag) - { - const NodeID to = facade->GetTarget(edge); - const int edge_weight = data.distance; + for (const auto edge : facade->GetAdjacentEdgeRange(node)) + { + const EdgeData &data = facade->GetEdgeData(edge); + const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward); + if (reverse_flag) + { + const NodeID to = facade->GetTarget(edge); + const int edge_weight = data.distance; - BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid"); + BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid"); - if (forward_heap.WasInserted(to)) - { - if (forward_heap.GetKey(to) + edge_weight < distance) - { - return; - } - } - } - } + if (forward_heap.WasInserted(to)) + { + if (forward_heap.GetKey(to) + edge_weight < distance) + { + return; + } + } + } + } } for (const auto edge : facade->GetAdjacentEdgeRange(node)) @@ -153,34 +172,34 @@ template class BasicRoutingInterface } } - void UnpackPath(const std::vector &packed_path, + template + void UnpackPath(RandomIter packed_path_begin, + RandomIter packed_path_end, const PhantomNodes &phantom_node_pair, std::vector &unpacked_path) const { const bool start_traversed_in_reverse = - (packed_path.front() != phantom_node_pair.source_phantom.forward_node_id); + (*packed_path_begin != phantom_node_pair.source_phantom.forward_node_id); const bool target_traversed_in_reverse = - (packed_path.back() != phantom_node_pair.target_phantom.forward_node_id); + (*std::prev(packed_path_end) != phantom_node_pair.target_phantom.forward_node_id); - const unsigned packed_path_size = static_cast(packed_path.size()); + const auto packed_path_size = std::distance(packed_path_begin, packed_path_end); + BOOST_ASSERT(packed_path_size > 0); std::stack> recursion_stack; // We have to push the path in reverse order onto the stack because it's LIFO. - for (unsigned i = packed_path_size - 1; i > 0; --i) + for (auto current = std::prev(packed_path_end); current != packed_path_begin; + current = std::prev(current)) { - recursion_stack.emplace(packed_path[i - 1], packed_path[i]); + recursion_stack.emplace(*std::prev(current), *current); } std::pair edge; while (!recursion_stack.empty()) { - /* - Graphical representation of variables: - - edge.first edge.second - *------------------>* - edge_id - */ + // edge.first edge.second + // *------------------>* + // edge_id edge = recursion_stack.top(); recursion_stack.pop(); @@ -199,13 +218,9 @@ template class BasicRoutingInterface } } - /* - Graphical representation of variables: - - edge.first edge.second - *<------------------* - edge_id - */ + // edge.first edge.second + // *<------------------* + // edge_id if (SPECIAL_EDGEID == smaller_edge_id) { for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.second)) @@ -306,12 +321,9 @@ template class BasicRoutingInterface { BOOST_ASSERT(i < id_vector.size()); BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0); - unpacked_path.emplace_back( - PathData{id_vector[i], - phantom_node_pair.target_phantom.name_id, - TurnInstruction::NoTurn, - 0, - phantom_node_pair.target_phantom.forward_travel_mode}); + unpacked_path.emplace_back(PathData{ + id_vector[i], phantom_node_pair.target_phantom.name_id, TurnInstruction::NoTurn, + 0, phantom_node_pair.target_phantom.forward_travel_mode}); } } @@ -419,71 +431,38 @@ template class BasicRoutingInterface // assumes that heaps are already setup correctly. void Search(SearchEngineData::QueryHeap &forward_heap, SearchEngineData::QueryHeap &reverse_heap, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, int &distance, std::vector &packed_leg) const { NodeID middle = SPECIAL_NODEID; - const EdgeWeight min_edge_offset = std::min(-source_phantom.GetForwardWeightPlusOffset(), - -source_phantom.GetReverseWeightPlusOffset()); - - // insert new starting nodes into forward heap, adjusted by previous distances. - if (source_phantom.forward_node_id != SPECIAL_NODEID) - { - forward_heap.Insert(source_phantom.forward_node_id, - -source_phantom.GetForwardWeightPlusOffset(), - source_phantom.forward_node_id); - } - if (source_phantom.reverse_node_id != SPECIAL_NODEID) - { - forward_heap.Insert(source_phantom.reverse_node_id, - -source_phantom.GetReverseWeightPlusOffset(), - source_phantom.reverse_node_id); - } - - // insert new backward nodes into backward heap, unadjusted. - if (target_phantom.forward_node_id != SPECIAL_NODEID) - { - reverse_heap.Insert(target_phantom.forward_node_id, - target_phantom.GetForwardWeightPlusOffset(), - target_phantom.forward_node_id); - } - - if (target_phantom.reverse_node_id != SPECIAL_NODEID) - { - reverse_heap.Insert(target_phantom.reverse_node_id, - target_phantom.GetReverseWeightPlusOffset(), - target_phantom.reverse_node_id); - } - - std::vector> forward_entry_points; - std::vector> reverse_entry_points; + // get offset to account for offsets on phantom nodes on compressed edges + const auto min_edge_offset = std::min(0, forward_heap.MinKey()); + BOOST_ASSERT(min_edge_offset <= 0); + // we only every insert negative offsets for nodes in the forward heap + BOOST_ASSERT(reverse_heap.MinKey() >= 0); // run two-Target Dijkstra routing step. while (0 < (forward_heap.Size() + reverse_heap.Size())) { if (!forward_heap.Empty()) { - RoutingStep(forward_heap, reverse_heap, &middle, &distance, min_edge_offset, - true); + RoutingStep(forward_heap, reverse_heap, middle, distance, min_edge_offset, true); } if (!reverse_heap.Empty()) { - RoutingStep(reverse_heap, forward_heap, &middle, &distance, min_edge_offset, - false); + RoutingStep(reverse_heap, forward_heap, middle, distance, min_edge_offset, false); } } // No path found for both target nodes? - if (INVALID_EDGE_WEIGHT == distance) + if (INVALID_EDGE_WEIGHT == distance || SPECIAL_NODEID == middle) { return; } // Was a paths over one of the forward/reverse nodes not found? - BOOST_ASSERT_MSG((SPECIAL_NODEID == middle || INVALID_EDGE_WEIGHT != distance), + BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != distance), "no path found"); RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg); @@ -494,48 +473,19 @@ template class BasicRoutingInterface SearchEngineData::QueryHeap &reverse_heap, SearchEngineData::QueryHeap &forward_core_heap, SearchEngineData::QueryHeap &reverse_core_heap, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, int &distance, std::vector &packed_leg) const { NodeID middle = SPECIAL_NODEID; - const EdgeWeight min_edge_offset = std::min(-source_phantom.GetForwardWeightPlusOffset(), - -source_phantom.GetReverseWeightPlusOffset()); - - // insert new starting nodes into forward heap, adjusted by previous distances. - if (source_phantom.forward_node_id != SPECIAL_NODEID) - { - forward_heap.Insert(source_phantom.forward_node_id, - -source_phantom.GetForwardWeightPlusOffset(), - source_phantom.forward_node_id); - } - if (source_phantom.reverse_node_id != SPECIAL_NODEID) - { - forward_heap.Insert(source_phantom.reverse_node_id, - -source_phantom.GetReverseWeightPlusOffset(), - source_phantom.reverse_node_id); - } - - // insert new backward nodes into backward heap, unadjusted. - if (target_phantom.forward_node_id != SPECIAL_NODEID) - { - reverse_heap.Insert(target_phantom.forward_node_id, - target_phantom.GetForwardWeightPlusOffset(), - target_phantom.forward_node_id); - } - - if (target_phantom.reverse_node_id != SPECIAL_NODEID) - { - reverse_heap.Insert(target_phantom.reverse_node_id, - target_phantom.GetReverseWeightPlusOffset(), - target_phantom.reverse_node_id); - } - std::vector> forward_entry_points; std::vector> reverse_entry_points; + // get offset to account for offsets on phantom nodes on compressed edges + const auto min_edge_offset = std::min(0, forward_heap.MinKey()); + // we only every insert negative offsets for nodes in the forward heap + BOOST_ASSERT(reverse_heap.MinKey() >= 0); + // run two-Target Dijkstra routing step. while (0 < (forward_heap.Size() + reverse_heap.Size())) { @@ -549,7 +499,7 @@ template class BasicRoutingInterface } else { - RoutingStep(forward_heap, reverse_heap, &middle, &distance, min_edge_offset, + RoutingStep(forward_heap, reverse_heap, middle, distance, min_edge_offset, true); } } @@ -563,7 +513,7 @@ template class BasicRoutingInterface } else { - RoutingStep(reverse_heap, forward_heap, &middle, &distance, min_edge_offset, + RoutingStep(reverse_heap, forward_heap, middle, distance, min_edge_offset, false); } } @@ -600,30 +550,42 @@ template class BasicRoutingInterface last_id = p.first; } + // get offset to account for offsets on phantom nodes on compressed edges + int min_core_edge_offset = 0; + if (forward_core_heap.Size() > 0) + { + min_core_edge_offset = std::min(min_core_edge_offset, forward_core_heap.MinKey()); + } + if (reverse_core_heap.Size() > 0 && reverse_core_heap.MinKey() < 0) + { + min_core_edge_offset = std::min(min_core_edge_offset, reverse_core_heap.MinKey()); + } + BOOST_ASSERT(min_core_edge_offset <= 0); + // run two-target Dijkstra routing step on core with termination criterion while (0 < (forward_core_heap.Size() + reverse_core_heap.Size()) && distance > (forward_core_heap.MinKey() + reverse_core_heap.MinKey())) { if (!forward_core_heap.Empty()) { - RoutingStep(forward_core_heap, reverse_core_heap, &middle, &distance, - min_edge_offset, true, false); + RoutingStep(forward_core_heap, reverse_core_heap, middle, distance, + min_core_edge_offset, true, false); } if (!reverse_core_heap.Empty()) { - RoutingStep(reverse_core_heap, forward_core_heap, &middle, &distance, - min_edge_offset, false, false); + RoutingStep(reverse_core_heap, forward_core_heap, middle, distance, + min_core_edge_offset, false, false); } } // No path found for both target nodes? - if (INVALID_EDGE_WEIGHT == distance) + if (INVALID_EDGE_WEIGHT == distance || SPECIAL_NODEID == middle) { return; } // Was a paths over one of the forward/reverse nodes not found? - BOOST_ASSERT_MSG((SPECIAL_NODEID == middle || INVALID_EDGE_WEIGHT != distance), + BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != distance), "no path found"); // we need to unpack sub path from core heaps @@ -685,12 +647,12 @@ template class BasicRoutingInterface { if (0 < forward_heap.Size()) { - RoutingStep(forward_heap, reverse_heap, &middle_node, &upper_bound, edge_offset, + RoutingStep(forward_heap, reverse_heap, middle_node, upper_bound, edge_offset, true); } if (0 < reverse_heap.Size()) { - RoutingStep(reverse_heap, forward_heap, &middle_node, &upper_bound, edge_offset, + RoutingStep(reverse_heap, forward_heap, middle_node, upper_bound, edge_offset, false); } } @@ -704,7 +666,7 @@ template class BasicRoutingInterface PhantomNodes nodes; nodes.source_phantom = source_phantom; nodes.target_phantom = target_phantom; - UnpackPath(packed_leg, nodes, unpacked_path); + UnpackPath(packed_leg.begin(), packed_leg.end(), nodes, unpacked_path); FixedPointCoordinate previous_coordinate = source_phantom.location; FixedPointCoordinate current_coordinate; @@ -713,11 +675,11 @@ template class BasicRoutingInterface { current_coordinate = facade->GetCoordinateOfNode(p.node); distance += coordinate_calculation::haversine_distance(previous_coordinate, - current_coordinate); + current_coordinate); previous_coordinate = current_coordinate; } distance += coordinate_calculation::haversine_distance(previous_coordinate, - target_phantom.location); + target_phantom.location); } return distance; } diff --git a/routing_algorithms/shortest_path.hpp b/routing_algorithms/shortest_path.hpp index eaa6d3c53..455e6b14c 100644 --- a/routing_algorithms/shortest_path.hpp +++ b/routing_algorithms/shortest_path.hpp @@ -28,12 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef SHORTEST_PATH_HPP #define SHORTEST_PATH_HPP -#include +#include "../typedefs.h" #include "routing_base.hpp" + #include "../data_structures/search_engine_data.hpp" #include "../util/integer_range.hpp" -#include "../typedefs.h" + +#include template class ShortestPathRouting final @@ -51,294 +53,482 @@ class ShortestPathRouting final ~ShortestPathRouting() {} + // allows a uturn at the target_phantom + // searches source forward/reverse -> target forward/reverse + void SearchWithUTurn(QueryHeap &forward_heap, + QueryHeap &reverse_heap, + const bool search_from_forward_node, + const bool search_from_reverse_node, + const bool search_to_forward_node, + const bool search_to_reverse_node, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + const int total_distance_to_forward, + const int total_distance_to_reverse, + int &new_total_distance, + std::vector &leg_packed_path) const + { + forward_heap.Clear(); + reverse_heap.Clear(); + if (search_from_forward_node) + { + forward_heap.Insert(source_phantom.forward_node_id, + total_distance_to_forward - + source_phantom.GetForwardWeightPlusOffset(), + source_phantom.forward_node_id); + } + if (search_from_reverse_node) + { + forward_heap.Insert(source_phantom.reverse_node_id, + total_distance_to_reverse - + source_phantom.GetReverseWeightPlusOffset(), + source_phantom.reverse_node_id); + } + if (search_to_forward_node) + { + reverse_heap.Insert(target_phantom.forward_node_id, + target_phantom.GetForwardWeightPlusOffset(), + target_phantom.forward_node_id); + } + if (search_to_reverse_node) + { + reverse_heap.Insert(target_phantom.reverse_node_id, + target_phantom.GetReverseWeightPlusOffset(), + target_phantom.reverse_node_id); + } + BOOST_ASSERT(forward_heap.Size() > 0); + BOOST_ASSERT(reverse_heap.Size() > 0); + super::Search(forward_heap, reverse_heap, new_total_distance, leg_packed_path); + } + + // If source and target are reverse on a oneway we need to find a path + // that connects the two. This is _not_ the shortest path in our model, + // as source and target are on the same edge based node. + // We force a detour by inserting "virtaul vias", which means we search a path + // from all nodes that are connected by outgoing edges to all nodes that are connected by + // incoming edges. + // ------^ + // | ^source + // | ^ + // | ^target + // ------^ + void SearchLoop(QueryHeap &forward_heap, + QueryHeap &reverse_heap, + const bool search_from_forward_node, + const bool search_from_reverse_node, + const bool search_to_forward_node, + const bool search_to_reverse_node, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + const int total_distance_to_forward, + const int total_distance_to_reverse, + int &new_total_distance_to_forward, + int &new_total_distance_to_reverse, + std::vector &leg_packed_path_forward, + std::vector &leg_packed_path_reverse) const + { + // silence unused warning + (void) search_from_forward_node; + (void) search_from_reverse_node; + + BOOST_ASSERT(search_from_reverse_node == search_to_reverse_node); + BOOST_ASSERT(search_from_forward_node == search_to_forward_node); + BOOST_ASSERT(source_phantom.forward_node_id == target_phantom.forward_node_id); + BOOST_ASSERT(source_phantom.reverse_node_id == target_phantom.reverse_node_id); + + if (search_to_forward_node) + { + forward_heap.Clear(); + reverse_heap.Clear(); + + auto node_id = source_phantom.forward_node_id; + + for (const auto edge : super::facade->GetAdjacentEdgeRange(node_id)) + { + const auto& data = super::facade->GetEdgeData(edge); + if (data.forward) + { + auto target = super::facade->GetTarget(edge); + auto offset = total_distance_to_forward + data.distance - source_phantom.GetForwardWeightPlusOffset(); + forward_heap.Insert(target, offset, target); + } + + if (data.backward) + { + auto target = super::facade->GetTarget(edge); + auto offset = data.distance + target_phantom.GetForwardWeightPlusOffset(); + reverse_heap.Insert(target, offset, target); + } + } + + BOOST_ASSERT(forward_heap.Size() > 0); + BOOST_ASSERT(reverse_heap.Size() > 0); + super::Search(forward_heap, reverse_heap, new_total_distance_to_forward, + leg_packed_path_forward); + + // insert node to both endpoints to close the leg + leg_packed_path_forward.push_back(node_id); + std::reverse(leg_packed_path_forward.begin(), leg_packed_path_forward.end()); + leg_packed_path_forward.push_back(node_id); + std::reverse(leg_packed_path_forward.begin(), leg_packed_path_forward.end()); + } + + if (search_to_reverse_node) + { + forward_heap.Clear(); + reverse_heap.Clear(); + + auto node_id = source_phantom.reverse_node_id; + + for (const auto edge : super::facade->GetAdjacentEdgeRange(node_id)) + { + const auto& data = super::facade->GetEdgeData(edge); + if (data.forward) + { + auto target = super::facade->GetTarget(edge); + auto offset = total_distance_to_reverse + data.distance - source_phantom.GetReverseWeightPlusOffset(); + forward_heap.Insert(target, offset, target); + } + + if (data.backward) + { + auto target = super::facade->GetTarget(edge); + auto offset = data.distance + target_phantom.GetReverseWeightPlusOffset(); + reverse_heap.Insert(target, offset, target); + } + } + + BOOST_ASSERT(forward_heap.Size() > 0); + BOOST_ASSERT(reverse_heap.Size() > 0); + super::Search(forward_heap, reverse_heap, new_total_distance_to_reverse, + leg_packed_path_reverse); + + // insert node to both endpoints to close the leg + leg_packed_path_reverse.push_back(node_id); + std::reverse(leg_packed_path_reverse.begin(), leg_packed_path_reverse.end()); + leg_packed_path_reverse.push_back(node_id); + std::reverse(leg_packed_path_reverse.begin(), leg_packed_path_reverse.end()); + } + + } + + // searches shortest path between: + // source forward/reverse -> target forward + // source forward/reverse -> target reverse + void Search(QueryHeap &forward_heap, + QueryHeap &reverse_heap, + const bool search_from_forward_node, + const bool search_from_reverse_node, + const bool search_to_forward_node, + const bool search_to_reverse_node, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + const int total_distance_to_forward, + const int total_distance_to_reverse, + int &new_total_distance_to_forward, + int &new_total_distance_to_reverse, + std::vector &leg_packed_path_forward, + std::vector &leg_packed_path_reverse) const + { + if (search_to_forward_node) + { + forward_heap.Clear(); + reverse_heap.Clear(); + reverse_heap.Insert(target_phantom.forward_node_id, + target_phantom.GetForwardWeightPlusOffset(), + target_phantom.forward_node_id); + + if (search_from_forward_node) + { + forward_heap.Insert(source_phantom.forward_node_id, + total_distance_to_forward - + source_phantom.GetForwardWeightPlusOffset(), + source_phantom.forward_node_id); + } + if (search_from_reverse_node) + { + forward_heap.Insert(source_phantom.reverse_node_id, + total_distance_to_reverse - + source_phantom.GetReverseWeightPlusOffset(), + source_phantom.reverse_node_id); + } + BOOST_ASSERT(forward_heap.Size() > 0); + BOOST_ASSERT(reverse_heap.Size() > 0); + super::Search(forward_heap, reverse_heap, new_total_distance_to_forward, + leg_packed_path_forward); + } + + if (search_to_reverse_node) + { + forward_heap.Clear(); + reverse_heap.Clear(); + reverse_heap.Insert(target_phantom.reverse_node_id, + target_phantom.GetReverseWeightPlusOffset(), + target_phantom.reverse_node_id); + if (search_from_forward_node) + { + forward_heap.Insert(source_phantom.forward_node_id, + total_distance_to_forward - + source_phantom.GetForwardWeightPlusOffset(), + source_phantom.forward_node_id); + } + if (search_from_reverse_node) + { + forward_heap.Insert(source_phantom.reverse_node_id, + total_distance_to_reverse - + source_phantom.GetReverseWeightPlusOffset(), + source_phantom.reverse_node_id); + } + BOOST_ASSERT(forward_heap.Size() > 0); + BOOST_ASSERT(reverse_heap.Size() > 0); + super::Search(forward_heap, reverse_heap, new_total_distance_to_reverse, + leg_packed_path_reverse); + } + } + + void UnpackLegs(const std::vector &phantom_nodes_vector, + const std::vector &total_packed_path, + const std::vector &packed_leg_begin, + const int shortest_path_length, + InternalRouteResult &raw_route_data) const + { + raw_route_data.unpacked_path_segments.resize(packed_leg_begin.size() - 1); + + raw_route_data.shortest_path_length = shortest_path_length; + + for (const auto current_leg : osrm::irange(0, packed_leg_begin.size() - 1)) + { + auto leg_begin = total_packed_path.begin() + packed_leg_begin[current_leg]; + auto leg_end = total_packed_path.begin() + packed_leg_begin[current_leg + 1]; + const auto &unpack_phantom_node_pair = phantom_nodes_vector[current_leg]; + super::UnpackPath(leg_begin, leg_end, unpack_phantom_node_pair, + raw_route_data.unpacked_path_segments[current_leg]); + + raw_route_data.source_traversed_in_reverse.push_back( + (*leg_begin != phantom_nodes_vector[current_leg].source_phantom.forward_node_id)); + raw_route_data.target_traversed_in_reverse.push_back( + (*std::prev(leg_end) != + phantom_nodes_vector[current_leg].target_phantom.forward_node_id)); + } + } + void operator()(const std::vector &phantom_nodes_vector, const std::vector &uturn_indicators, InternalRouteResult &raw_route_data) const { - int distance1 = 0; - int distance2 = 0; - bool search_from_1st_node = true; - bool search_from_2nd_node = true; - NodeID middle1 = SPECIAL_NODEID; - NodeID middle2 = SPECIAL_NODEID; - std::vector> packed_legs1(phantom_nodes_vector.size()); - std::vector> packed_legs2(phantom_nodes_vector.size()); - engine_working_data.InitializeOrClearFirstThreadLocalStorage( super::facade->GetNumberOfNodes()); - engine_working_data.InitializeOrClearSecondThreadLocalStorage( - super::facade->GetNumberOfNodes()); - engine_working_data.InitializeOrClearThirdThreadLocalStorage( - super::facade->GetNumberOfNodes()); - QueryHeap &forward_heap1 = *(engine_working_data.forward_heap_1); - QueryHeap &reverse_heap1 = *(engine_working_data.reverse_heap_1); - QueryHeap &forward_heap2 = *(engine_working_data.forward_heap_2); - QueryHeap &reverse_heap2 = *(engine_working_data.reverse_heap_2); + QueryHeap &forward_heap = *(engine_working_data.forward_heap_1); + QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1); + + int total_distance_to_forward = 0; + int total_distance_to_reverse = 0; + bool search_from_forward_node = phantom_nodes_vector.front().source_phantom.forward_node_id != SPECIAL_NODEID; + bool search_from_reverse_node = phantom_nodes_vector.front().source_phantom.reverse_node_id != SPECIAL_NODEID; + + std::vector prev_packed_leg_to_forward; + std::vector prev_packed_leg_to_reverse; + + std::vector total_packed_path_to_forward; + std::vector packed_leg_to_forward_begin; + std::vector total_packed_path_to_reverse; + std::vector packed_leg_to_reverse_begin; std::size_t current_leg = 0; - // Get distance to next pair of target nodes. - for (const PhantomNodes &phantom_node_pair : phantom_nodes_vector) + // this implements a dynamic program that finds the shortest route through + // a list of vias + for (const auto &phantom_node_pair : phantom_nodes_vector) { - forward_heap1.Clear(); - forward_heap2.Clear(); - reverse_heap1.Clear(); - reverse_heap2.Clear(); - int local_upper_bound1 = INVALID_EDGE_WEIGHT; - int local_upper_bound2 = INVALID_EDGE_WEIGHT; + int new_total_distance_to_forward = INVALID_EDGE_WEIGHT; + int new_total_distance_to_reverse = INVALID_EDGE_WEIGHT; - middle1 = SPECIAL_NODEID; - middle2 = SPECIAL_NODEID; + std::vector packed_leg_to_forward; + std::vector packed_leg_to_reverse; - const bool allow_u_turn = current_leg > 0 && uturn_indicators.size() > current_leg && - uturn_indicators[current_leg - 1]; - const EdgeWeight min_edge_offset = - std::min(-phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(), - -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset()); + const auto &source_phantom = phantom_node_pair.source_phantom; + const auto &target_phantom = phantom_node_pair.target_phantom; - // insert new starting nodes into forward heap, adjusted by previous distances. - if ((allow_u_turn || search_from_1st_node) && - phantom_node_pair.source_phantom.forward_node_id != SPECIAL_NODEID) + + const bool allow_u_turn_at_via = uturn_indicators.size() > current_leg && + uturn_indicators[current_leg]; + + bool search_to_forward_node = target_phantom.forward_node_id != SPECIAL_NODEID; + bool search_to_reverse_node = target_phantom.reverse_node_id != SPECIAL_NODEID; + + BOOST_ASSERT(!search_from_forward_node || source_phantom.forward_node_id != SPECIAL_NODEID); + BOOST_ASSERT(!search_from_reverse_node || source_phantom.reverse_node_id != SPECIAL_NODEID); + + + if (source_phantom.forward_node_id == target_phantom.forward_node_id && + source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset()) { - forward_heap1.Insert( - phantom_node_pair.source_phantom.forward_node_id, - -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(), - phantom_node_pair.source_phantom.forward_node_id); - // SimpleLogger().Write(logDEBUG) << "fwd-a2 insert: " << - // phantom_node_pair.source_phantom.forward_node_id << ", w: " << -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(); - forward_heap2.Insert( - phantom_node_pair.source_phantom.forward_node_id, - -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(), - phantom_node_pair.source_phantom.forward_node_id); - // SimpleLogger().Write(logDEBUG) << "fwd-b2 insert: " << - // phantom_node_pair.source_phantom.forward_node_id << ", w: " << -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(); + search_to_forward_node = false; } - if ((allow_u_turn || search_from_2nd_node) && - phantom_node_pair.source_phantom.reverse_node_id != SPECIAL_NODEID) + if (source_phantom.reverse_node_id == target_phantom.reverse_node_id && + source_phantom.GetReverseWeightPlusOffset() > target_phantom.GetReverseWeightPlusOffset()) { - forward_heap1.Insert( - phantom_node_pair.source_phantom.reverse_node_id, - -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(), - phantom_node_pair.source_phantom.reverse_node_id); - // SimpleLogger().Write(logDEBUG) << "fwd-a2 insert: " << - // phantom_node_pair.source_phantom.reverse_node_id << ", w: " << -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(); - forward_heap2.Insert( - phantom_node_pair.source_phantom.reverse_node_id, - -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(), - phantom_node_pair.source_phantom.reverse_node_id); - // SimpleLogger().Write(logDEBUG) << "fwd-b2 insert: " << - // phantom_node_pair.source_phantom.reverse_node_id << ", w: " << -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(); + search_to_reverse_node = false; } - // insert new backward nodes into backward heap, unadjusted. - if (phantom_node_pair.target_phantom.forward_node_id != SPECIAL_NODEID) - { - reverse_heap1.Insert(phantom_node_pair.target_phantom.forward_node_id, - phantom_node_pair.target_phantom.GetForwardWeightPlusOffset(), - phantom_node_pair.target_phantom.forward_node_id); - // SimpleLogger().Write(logDEBUG) << "rev-a insert: " << - // phantom_node_pair.target_phantom.forward_node_id << ", w: " << phantom_node_pair.target_phantom.GetForwardWeightPlusOffset(); - } + BOOST_ASSERT(search_from_forward_node || search_from_reverse_node); - if (phantom_node_pair.target_phantom.reverse_node_id != SPECIAL_NODEID) + if(search_to_reverse_node || search_to_forward_node) { - reverse_heap2.Insert(phantom_node_pair.target_phantom.reverse_node_id, - phantom_node_pair.target_phantom.GetReverseWeightPlusOffset(), - phantom_node_pair.target_phantom.reverse_node_id); - // SimpleLogger().Write(logDEBUG) << "rev-a insert: " << - // phantom_node_pair.target_phantom.reverse_node_id << ", w: " << phantom_node_pair.target_phantom.GetReverseWeightPlusOffset(); - } - - // run two-Target Dijkstra routing step. - while (0 < (forward_heap1.Size() + reverse_heap1.Size())) - { - if (!forward_heap1.Empty()) + if (allow_u_turn_at_via) { - super::RoutingStep(forward_heap1, reverse_heap1, &middle1, &local_upper_bound1, - min_edge_offset, true); - } - if (!reverse_heap1.Empty()) - { - super::RoutingStep(reverse_heap1, forward_heap1, &middle1, &local_upper_bound1, - min_edge_offset, false); - } - } - - if (!reverse_heap2.Empty()) - { - while (0 < (forward_heap2.Size() + reverse_heap2.Size())) - { - if (!forward_heap2.Empty()) + SearchWithUTurn(forward_heap, reverse_heap, search_from_forward_node, + search_from_reverse_node, search_to_forward_node, + search_to_reverse_node, source_phantom, target_phantom, + total_distance_to_forward, total_distance_to_reverse, + new_total_distance_to_forward, packed_leg_to_forward); + if (target_phantom.reverse_node_id != SPECIAL_NODEID) { - super::RoutingStep(forward_heap2, reverse_heap2, &middle2, - &local_upper_bound2, min_edge_offset, true); - } - if (!reverse_heap2.Empty()) - { - super::RoutingStep(reverse_heap2, forward_heap2, &middle2, - &local_upper_bound2, min_edge_offset, false); + new_total_distance_to_reverse = new_total_distance_to_forward; + packed_leg_to_reverse = packed_leg_to_forward; } } + else + { + Search(forward_heap, reverse_heap, search_from_forward_node, + search_from_reverse_node, search_to_forward_node, search_to_reverse_node, + source_phantom, target_phantom, total_distance_to_forward, + total_distance_to_reverse, new_total_distance_to_forward, + new_total_distance_to_reverse, packed_leg_to_forward, packed_leg_to_reverse); + } + } + else + { + search_to_forward_node = target_phantom.forward_node_id != SPECIAL_NODEID; + search_to_reverse_node = target_phantom.reverse_node_id != SPECIAL_NODEID; + SearchLoop(forward_heap, reverse_heap, search_from_forward_node, + search_from_reverse_node, search_to_forward_node, search_to_reverse_node, + source_phantom, target_phantom, total_distance_to_forward, + total_distance_to_reverse, new_total_distance_to_forward, + new_total_distance_to_reverse, packed_leg_to_forward, packed_leg_to_reverse); } // No path found for both target nodes? - if ((INVALID_EDGE_WEIGHT == local_upper_bound1) && - (INVALID_EDGE_WEIGHT == local_upper_bound2)) + if ((INVALID_EDGE_WEIGHT == new_total_distance_to_forward) && + (INVALID_EDGE_WEIGHT == new_total_distance_to_reverse)) { raw_route_data.shortest_path_length = INVALID_EDGE_WEIGHT; raw_route_data.alternative_path_length = INVALID_EDGE_WEIGHT; return; } - search_from_1st_node = true; - search_from_2nd_node = true; - if (SPECIAL_NODEID == middle1) + // we need to figure out how the new legs connect to the previous ones + if (current_leg > 0) { - search_from_1st_node = false; - } - if (SPECIAL_NODEID == middle2) - { - search_from_2nd_node = false; - } + bool forward_to_forward = + (new_total_distance_to_forward != INVALID_EDGE_WEIGHT) && + packed_leg_to_forward.front() == source_phantom.forward_node_id; + bool reverse_to_forward = + (new_total_distance_to_forward != INVALID_EDGE_WEIGHT) && + packed_leg_to_forward.front() == source_phantom.reverse_node_id; + bool forward_to_reverse = + (new_total_distance_to_reverse != INVALID_EDGE_WEIGHT) && + packed_leg_to_reverse.front() == source_phantom.forward_node_id; + bool reverse_to_reverse = + (new_total_distance_to_reverse != INVALID_EDGE_WEIGHT) && + packed_leg_to_reverse.front() == source_phantom.reverse_node_id; - // Was at most one of the two paths not found? - BOOST_ASSERT_MSG((INVALID_EDGE_WEIGHT != distance1 || INVALID_EDGE_WEIGHT != distance2), - "no path found"); + BOOST_ASSERT(!forward_to_forward || !reverse_to_forward); + BOOST_ASSERT(!forward_to_reverse || !reverse_to_reverse); - // Unpack paths if they exist - std::vector temporary_packed_leg1; - std::vector temporary_packed_leg2; - - BOOST_ASSERT(current_leg < packed_legs1.size()); - BOOST_ASSERT(current_leg < packed_legs2.size()); - - if (INVALID_EDGE_WEIGHT != local_upper_bound1) - { - super::RetrievePackedPathFromHeap(forward_heap1, reverse_heap1, middle1, - temporary_packed_leg1); - } - - if (INVALID_EDGE_WEIGHT != local_upper_bound2) - { - super::RetrievePackedPathFromHeap(forward_heap2, reverse_heap2, middle2, - temporary_packed_leg2); - } - - // if one of the paths was not found, replace it with the other one. - if ((allow_u_turn && local_upper_bound1 > local_upper_bound2) || - temporary_packed_leg1.empty()) - { - temporary_packed_leg1.clear(); - temporary_packed_leg1.insert(temporary_packed_leg1.end(), - temporary_packed_leg2.begin(), - temporary_packed_leg2.end()); - local_upper_bound1 = local_upper_bound2; - } - if ((allow_u_turn && local_upper_bound2 > local_upper_bound1) || - temporary_packed_leg2.empty()) - { - temporary_packed_leg2.clear(); - temporary_packed_leg2.insert(temporary_packed_leg2.end(), - temporary_packed_leg1.begin(), - temporary_packed_leg1.end()); - local_upper_bound2 = local_upper_bound1; - } - - BOOST_ASSERT_MSG(!temporary_packed_leg1.empty() || !temporary_packed_leg2.empty(), - "tempory packed paths empty"); - - BOOST_ASSERT((0 == current_leg) || !packed_legs1[current_leg - 1].empty()); - BOOST_ASSERT((0 == current_leg) || !packed_legs2[current_leg - 1].empty()); - - if (!allow_u_turn && 0 < current_leg) - { - const NodeID end_id_of_segment1 = packed_legs1[current_leg - 1].back(); - const NodeID end_id_of_segment2 = packed_legs2[current_leg - 1].back(); - BOOST_ASSERT(!temporary_packed_leg1.empty()); - const NodeID start_id_of_leg1 = temporary_packed_leg1.front(); - const NodeID start_id_of_leg2 = temporary_packed_leg2.front(); - if ((end_id_of_segment1 != start_id_of_leg1) && - (end_id_of_segment2 != start_id_of_leg2)) + // in this case we always need to copy + if (forward_to_forward && forward_to_reverse) { - std::swap(temporary_packed_leg1, temporary_packed_leg2); - std::swap(local_upper_bound1, local_upper_bound2); + // in this case we copy the path leading to the source forward node + // and change the case + total_packed_path_to_reverse = total_packed_path_to_forward; + packed_leg_to_reverse_begin = packed_leg_to_forward_begin; + forward_to_reverse = false; + reverse_to_reverse = true; } - - // remove the shorter path if both legs end at the same segment - if (start_id_of_leg1 == start_id_of_leg2) + else if (reverse_to_forward && reverse_to_reverse) { - const NodeID last_id_of_packed_legs1 = packed_legs1[current_leg - 1].back(); - const NodeID last_id_of_packed_legs2 = packed_legs2[current_leg - 1].back(); - if (start_id_of_leg1 != last_id_of_packed_legs1) - { - packed_legs1 = packed_legs2; - distance1 = distance2; - BOOST_ASSERT(start_id_of_leg1 == temporary_packed_leg1.front()); - } - else if (start_id_of_leg2 != last_id_of_packed_legs2) - { - packed_legs2 = packed_legs1; - distance2 = distance1; - BOOST_ASSERT(start_id_of_leg2 == temporary_packed_leg2.front()); - } + total_packed_path_to_forward = total_packed_path_to_reverse; + packed_leg_to_forward_begin = packed_leg_to_reverse_begin; + reverse_to_forward = false; + forward_to_forward = true; + } + BOOST_ASSERT(!forward_to_forward || !forward_to_reverse); + BOOST_ASSERT(!reverse_to_forward || !reverse_to_reverse); + + // in this case we just need to swap to regain the correct mapping + if (reverse_to_forward || forward_to_reverse) + { + total_packed_path_to_forward.swap(total_packed_path_to_reverse); + packed_leg_to_forward_begin.swap(packed_leg_to_reverse_begin); } } - BOOST_ASSERT(packed_legs1.size() == packed_legs2.size()); - packed_legs1[current_leg].insert(packed_legs1[current_leg].end(), - temporary_packed_leg1.begin(), - temporary_packed_leg1.end()); - BOOST_ASSERT(packed_legs1[current_leg].size() == temporary_packed_leg1.size()); - packed_legs2[current_leg].insert(packed_legs2[current_leg].end(), - temporary_packed_leg2.begin(), - temporary_packed_leg2.end()); - BOOST_ASSERT(packed_legs2[current_leg].size() == temporary_packed_leg2.size()); - - if (!allow_u_turn && - (packed_legs1[current_leg].back() == packed_legs2[current_leg].back()) && - phantom_node_pair.target_phantom.is_bidirected()) + if (new_total_distance_to_forward != INVALID_EDGE_WEIGHT) { - const NodeID last_node_id = packed_legs2[current_leg].back(); - search_from_1st_node &= - !(last_node_id == phantom_node_pair.target_phantom.reverse_node_id); - search_from_2nd_node &= - !(last_node_id == phantom_node_pair.target_phantom.forward_node_id); - BOOST_ASSERT(search_from_1st_node != search_from_2nd_node); + BOOST_ASSERT(target_phantom.forward_node_id != SPECIAL_NODEID); + + packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size()); + total_packed_path_to_forward.insert(total_packed_path_to_forward.end(), + packed_leg_to_forward.begin(), + packed_leg_to_forward.end()); + search_from_forward_node = true; + } + else + { + total_packed_path_to_forward.clear(); + packed_leg_to_forward_begin.clear(); + search_from_forward_node = false; } - distance1 += local_upper_bound1; - distance2 += local_upper_bound2; + if (new_total_distance_to_reverse != INVALID_EDGE_WEIGHT) + { + BOOST_ASSERT(target_phantom.reverse_node_id != SPECIAL_NODEID); + + packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size()); + total_packed_path_to_reverse.insert(total_packed_path_to_reverse.end(), + packed_leg_to_reverse.begin(), + packed_leg_to_reverse.end()); + search_from_reverse_node = true; + } + else + { + total_packed_path_to_reverse.clear(); + packed_leg_to_reverse_begin.clear(); + search_from_reverse_node = false; + } + + prev_packed_leg_to_forward = std::move(packed_leg_to_forward); + prev_packed_leg_to_reverse = std::move(packed_leg_to_reverse); + + total_distance_to_forward = new_total_distance_to_forward; + total_distance_to_reverse = new_total_distance_to_reverse; + ++current_leg; } - if (distance1 > distance2) + BOOST_ASSERT(total_distance_to_forward != INVALID_EDGE_WEIGHT || + total_distance_to_reverse != INVALID_EDGE_WEIGHT); + + // We make sure the fastest route is always in packed_legs_to_forward + if (total_distance_to_forward > total_distance_to_reverse) { - std::swap(packed_legs1, packed_legs2); - } - raw_route_data.unpacked_path_segments.resize(packed_legs1.size()); + // insert sentinel + packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size()); + BOOST_ASSERT(packed_leg_to_reverse_begin.size() == phantom_nodes_vector.size() + 1); - for (const std::size_t index : osrm::irange(0, packed_legs1.size())) + UnpackLegs(phantom_nodes_vector, total_packed_path_to_reverse, + packed_leg_to_reverse_begin, total_distance_to_reverse, raw_route_data); + } + else { - BOOST_ASSERT(!phantom_nodes_vector.empty()); - BOOST_ASSERT(packed_legs1.size() == raw_route_data.unpacked_path_segments.size()); + // insert sentinel + packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size()); + BOOST_ASSERT(packed_leg_to_forward_begin.size() == phantom_nodes_vector.size() + 1); - PhantomNodes unpack_phantom_node_pair = phantom_nodes_vector[index]; - super::UnpackPath( - // -- packed input - packed_legs1[index], - // -- start and end of (sub-)route - unpack_phantom_node_pair, - // -- unpacked output - raw_route_data.unpacked_path_segments[index]); - - raw_route_data.source_traversed_in_reverse.push_back( - (packed_legs1[index].front() != - phantom_nodes_vector[index].source_phantom.forward_node_id)); - raw_route_data.target_traversed_in_reverse.push_back( - (packed_legs1[index].back() != - phantom_nodes_vector[index].target_phantom.forward_node_id)); + UnpackLegs(phantom_nodes_vector, total_packed_path_to_forward, + packed_leg_to_forward_begin, total_distance_to_forward, raw_route_data); } - raw_route_data.shortest_path_length = std::min(distance1, distance2); } };