Open-Harbor/osrm-backend
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

Refactor direct_shortest_path and shortest_path

Browse Source
This commit is contained in:
Patrick Niklaus 2015-12-01 05:28:57 +01:00
parent 6d2a65b4ea
commit 019e26dd8e
5 changed files with 602 additions and 413 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
plugins/viaroute.hpp
View File

@ -114,6 +114,12 @@ template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
{ {
phantom_node_pair_list[i].second = phantom_node_vector.back(); 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;
} }
} }

26
routing_algorithms/alternative_path.hpp
View File

@ -317,7 +317,7 @@ class AlternativeRouting final
super::UnpackPath( super::UnpackPath(
// -- packed input // -- packed input
packed_shortest_path, packed_shortest_path.begin(), packed_shortest_path.end(),
// -- start of route // -- start of route
phantom_node_pair, phantom_node_pair,
// -- unpacked output // -- unpacked output
@ -338,8 +338,8 @@ class AlternativeRouting final
(packed_alternate_path.back() != phantom_node_pair.target_phantom.forward_node_id)); (packed_alternate_path.back() != phantom_node_pair.target_phantom.forward_node_id));
// unpack the alternate path // unpack the alternate path
super::UnpackPath(packed_alternate_path, phantom_node_pair, super::UnpackPath(packed_alternate_path.begin(), packed_alternate_path.end(),
raw_route_data.unpacked_alternative); phantom_node_pair, raw_route_data.unpacked_alternative);
raw_route_data.alternative_path_length = length_of_via_path; raw_route_data.alternative_path_length = length_of_via_path;
} }
@ -401,8 +401,8 @@ class AlternativeRouting final
// compute path <s,..,v> by reusing forward search from s // compute path <s,..,v> by reusing forward search from s
while (!new_reverse_heap.Empty()) while (!new_reverse_heap.Empty())
{ {
super::RoutingStep(new_reverse_heap, existing_forward_heap, &s_v_middle, super::RoutingStep(new_reverse_heap, existing_forward_heap, s_v_middle,
&upper_bound_s_v_path_length, min_edge_offset, false); upper_bound_s_v_path_length, min_edge_offset, false);
} }
// compute path <v,..,t> by reusing backward search from node t // compute path <v,..,t> by reusing backward search from node t
NodeID v_t_middle = SPECIAL_NODEID; NodeID v_t_middle = SPECIAL_NODEID;
@ -410,8 +410,8 @@ class AlternativeRouting final
new_forward_heap.Insert(via_node, 0, via_node); new_forward_heap.Insert(via_node, 0, via_node);
while (!new_forward_heap.Empty()) while (!new_forward_heap.Empty())
{ {
super::RoutingStep(new_forward_heap, existing_reverse_heap, &v_t_middle, super::RoutingStep(new_forward_heap, existing_reverse_heap, v_t_middle,
&upper_bound_of_v_t_path_length, min_edge_offset, true); 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; *real_length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length;
@ -671,8 +671,8 @@ class AlternativeRouting final
new_reverse_heap.Insert(candidate.node, 0, candidate.node); new_reverse_heap.Insert(candidate.node, 0, candidate.node);
while (new_reverse_heap.Size() > 0) while (new_reverse_heap.Size() > 0)
{ {
super::RoutingStep(new_reverse_heap, existing_forward_heap, s_v_middle, super::RoutingStep(new_reverse_heap, existing_forward_heap, *s_v_middle,
&upper_bound_s_v_path_length, min_edge_offset, false); upper_bound_s_v_path_length, min_edge_offset, false);
} }
if (INVALID_EDGE_WEIGHT == upper_bound_s_v_path_length) 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); new_forward_heap.Insert(candidate.node, 0, candidate.node);
while (new_forward_heap.Size() > 0) while (new_forward_heap.Size() > 0)
{ {
super::RoutingStep(new_forward_heap, existing_reverse_heap, v_t_middle, super::RoutingStep(new_forward_heap, existing_reverse_heap, *v_t_middle,
&upper_bound_of_v_t_path_length, min_edge_offset, true); upper_bound_of_v_t_path_length, min_edge_offset, true);
} }
if (INVALID_EDGE_WEIGHT == upper_bound_of_v_t_path_length) if (INVALID_EDGE_WEIGHT == upper_bound_of_v_t_path_length)
@ -854,12 +854,12 @@ class AlternativeRouting final
{ {
if (!forward_heap3.Empty()) 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); min_edge_offset, true);
} }
if (!reverse_heap3.Empty()) 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); min_edge_offset, false);
} }
} }

41
routing_algorithms/direct_shortest_path.hpp
View File

@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define DIRECT_SHORTEST_PATH_HPP #define DIRECT_SHORTEST_PATH_HPP
#include <boost/assert.hpp> #include <boost/assert.hpp>
#include <iterator>
#include "routing_base.hpp" #include "routing_base.hpp"
#include "../data_structures/search_engine_data.hpp" #include "../data_structures/search_engine_data.hpp"
@ -68,6 +69,8 @@ class DirectShortestPathRouting final
BOOST_ASSERT_MSG(1 == phantom_nodes_vector.size(), 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."); "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& 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( engine_working_data.InitializeOrClearFirstThreadLocalStorage(
super::facade->GetNumberOfNodes()); super::facade->GetNumberOfNodes());
@ -76,7 +79,37 @@ class DirectShortestPathRouting final
forward_heap.Clear(); forward_heap.Clear();
reverse_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<NodeID> packed_leg; std::vector<NodeID> packed_leg;
if (super::facade->GetCoreSize() > 0) if (super::facade->GetCoreSize() > 0)
@ -90,13 +123,11 @@ class DirectShortestPathRouting final
super::SearchWithCore(forward_heap, reverse_heap, forward_core_heap, reverse_core_heap, 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); distance, packed_leg);
} }
else else
{ {
super::Search(forward_heap, reverse_heap, phantom_node_pair.source_phantom, super::Search(forward_heap, reverse_heap, distance, packed_leg);
phantom_node_pair.target_phantom, distance, packed_leg);
} }
// No path found for both target nodes? // No path found for both target nodes?
@ -116,7 +147,7 @@ class DirectShortestPathRouting final
raw_route_data.target_traversed_in_reverse.push_back( raw_route_data.target_traversed_in_reverse.push_back(
(packed_leg.back() != phantom_node_pair.target_phantom.forward_node_id)); (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());
} }
}; };

226
routing_algorithms/routing_base.hpp
View File

@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "../data_structures/internal_route_result.hpp" #include "../data_structures/internal_route_result.hpp"
#include "../data_structures/search_engine_data.hpp" #include "../data_structures/search_engine_data.hpp"
#include "../data_structures/turn_instructions.hpp" #include "../data_structures/turn_instructions.hpp"
// #include "../util/simple_logger.hpp"
#include <boost/assert.hpp> #include <boost/assert.hpp>
@ -59,42 +58,62 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
explicit BasicRoutingInterface(DataFacadeT *facade) : facade(facade) {} explicit BasicRoutingInterface(DataFacadeT *facade) : facade(facade) {}
~BasicRoutingInterface() {} ~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, void RoutingStep(SearchEngineData::QueryHeap &forward_heap,
SearchEngineData::QueryHeap &reverse_heap, SearchEngineData::QueryHeap &reverse_heap,
NodeID *middle_node_id, NodeID &middle_node_id,
int *upper_bound, int &upper_bound,
const int min_edge_offset, int min_edge_offset,
const bool forward_direction, const bool forward_direction,
const bool stalling=true) const const bool stalling = true) const
{ {
const NodeID node = forward_heap.DeleteMin(); const NodeID node = forward_heap.DeleteMin();
const int distance = forward_heap.GetKey(node); 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)) if (reverse_heap.WasInserted(node))
{ {
const int new_distance = reverse_heap.GetKey(node) + distance; const int new_distance = reverse_heap.GetKey(node) + distance;
if (new_distance < *upper_bound) if (new_distance < upper_bound)
{ {
if (new_distance >= 0) if (new_distance >= 0)
{ {
*middle_node_id = node; middle_node_id = node;
*upper_bound = new_distance; 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;
} }
} }
} }
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(); forward_heap.DeleteAll();
return; return;
} }
@ -153,34 +172,34 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
} }
} }
void UnpackPath(const std::vector<NodeID> &packed_path, template <typename RandomIter>
void UnpackPath(RandomIter packed_path_begin,
RandomIter packed_path_end,
const PhantomNodes &phantom_node_pair, const PhantomNodes &phantom_node_pair,
std::vector<PathData> &unpacked_path) const std::vector<PathData> &unpacked_path) const
{ {
const bool start_traversed_in_reverse = 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 = 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<unsigned>(packed_path.size()); const auto packed_path_size = std::distance(packed_path_begin, packed_path_end);
BOOST_ASSERT(packed_path_size > 0);
std::stack<std::pair<NodeID, NodeID>> recursion_stack; std::stack<std::pair<NodeID, NodeID>> recursion_stack;
// We have to push the path in reverse order onto the stack because it's LIFO. // 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<NodeID, NodeID> edge; std::pair<NodeID, NodeID> edge;
while (!recursion_stack.empty()) while (!recursion_stack.empty())
{ {
/* // edge.first edge.second
Graphical representation of variables: // *------------------>*
// edge_id
edge.first edge.second
*------------------>*
edge_id
*/
edge = recursion_stack.top(); edge = recursion_stack.top();
recursion_stack.pop(); recursion_stack.pop();
@ -199,13 +218,9 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
} }
} }
/* // edge.first edge.second
Graphical representation of variables: // *<------------------*
// edge_id
edge.first edge.second
*<------------------*
edge_id
*/
if (SPECIAL_EDGEID == smaller_edge_id) if (SPECIAL_EDGEID == smaller_edge_id)
{ {
for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.second)) for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.second))
@ -306,12 +321,9 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
{ {
BOOST_ASSERT(i < id_vector.size()); BOOST_ASSERT(i < id_vector.size());
BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0); BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0);
unpacked_path.emplace_back( unpacked_path.emplace_back(PathData{
PathData{id_vector[i], id_vector[i], phantom_node_pair.target_phantom.name_id, TurnInstruction::NoTurn,
phantom_node_pair.target_phantom.name_id, 0, phantom_node_pair.target_phantom.forward_travel_mode});
TurnInstruction::NoTurn,
0,
phantom_node_pair.target_phantom.forward_travel_mode});
} }
} }
@ -419,71 +431,38 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
// assumes that heaps are already setup correctly. // assumes that heaps are already setup correctly.
void Search(SearchEngineData::QueryHeap &forward_heap, void Search(SearchEngineData::QueryHeap &forward_heap,
SearchEngineData::QueryHeap &reverse_heap, SearchEngineData::QueryHeap &reverse_heap,
const PhantomNode &source_phantom,
const PhantomNode &target_phantom,
int &distance, int &distance,
std::vector<NodeID> &packed_leg) const std::vector<NodeID> &packed_leg) const
{ {
NodeID middle = SPECIAL_NODEID; NodeID middle = SPECIAL_NODEID;
const EdgeWeight min_edge_offset = std::min(-source_phantom.GetForwardWeightPlusOffset(), // get offset to account for offsets on phantom nodes on compressed edges
-source_phantom.GetReverseWeightPlusOffset()); const auto min_edge_offset = std::min(0, forward_heap.MinKey());
BOOST_ASSERT(min_edge_offset <= 0);
// insert new starting nodes into forward heap, adjusted by previous distances. // we only every insert negative offsets for nodes in the forward heap
if (source_phantom.forward_node_id != SPECIAL_NODEID) BOOST_ASSERT(reverse_heap.MinKey() >= 0);
{
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<std::pair<NodeID, EdgeWeight>> forward_entry_points;
std::vector<std::pair<NodeID, EdgeWeight>> reverse_entry_points;
// run two-Target Dijkstra routing step. // run two-Target Dijkstra routing step.
while (0 < (forward_heap.Size() + reverse_heap.Size())) while (0 < (forward_heap.Size() + reverse_heap.Size()))
{ {
if (!forward_heap.Empty()) if (!forward_heap.Empty())
{ {
RoutingStep(forward_heap, reverse_heap, &middle, &distance, min_edge_offset, RoutingStep(forward_heap, reverse_heap, middle, distance, min_edge_offset, true);
true);
} }
if (!reverse_heap.Empty()) if (!reverse_heap.Empty())
{ {
RoutingStep(reverse_heap, forward_heap, &middle, &distance, min_edge_offset, RoutingStep(reverse_heap, forward_heap, middle, distance, min_edge_offset, false);
false);
} }
} }
// No path found for both target nodes? // No path found for both target nodes?
if (INVALID_EDGE_WEIGHT == distance) if (INVALID_EDGE_WEIGHT == distance || SPECIAL_NODEID == middle)
{ {
return; return;
} }
// Was a paths over one of the forward/reverse nodes not found? // 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"); "no path found");
RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg); RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
@ -494,48 +473,19 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
SearchEngineData::QueryHeap &reverse_heap, SearchEngineData::QueryHeap &reverse_heap,
SearchEngineData::QueryHeap &forward_core_heap, SearchEngineData::QueryHeap &forward_core_heap,
SearchEngineData::QueryHeap &reverse_core_heap, SearchEngineData::QueryHeap &reverse_core_heap,
const PhantomNode &source_phantom,
const PhantomNode &target_phantom,
int &distance, int &distance,
std::vector<NodeID> &packed_leg) const std::vector<NodeID> &packed_leg) const
{ {
NodeID middle = SPECIAL_NODEID; 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<std::pair<NodeID, EdgeWeight>> forward_entry_points; std::vector<std::pair<NodeID, EdgeWeight>> forward_entry_points;
std::vector<std::pair<NodeID, EdgeWeight>> reverse_entry_points; std::vector<std::pair<NodeID, EdgeWeight>> 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. // run two-Target Dijkstra routing step.
while (0 < (forward_heap.Size() + reverse_heap.Size())) while (0 < (forward_heap.Size() + reverse_heap.Size()))
{ {
@ -549,7 +499,7 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
} }
else else
{ {
RoutingStep(forward_heap, reverse_heap, &middle, &distance, min_edge_offset, RoutingStep(forward_heap, reverse_heap, middle, distance, min_edge_offset,
true); true);
} }
} }
@ -563,7 +513,7 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
} }
else else
{ {
RoutingStep(reverse_heap, forward_heap, &middle, &distance, min_edge_offset, RoutingStep(reverse_heap, forward_heap, middle, distance, min_edge_offset,
false); false);
} }
} }
@ -600,30 +550,42 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
last_id = p.first; 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 // run two-target Dijkstra routing step on core with termination criterion
while (0 < (forward_core_heap.Size() + reverse_core_heap.Size()) && while (0 < (forward_core_heap.Size() + reverse_core_heap.Size()) &&
distance > (forward_core_heap.MinKey() + reverse_core_heap.MinKey())) distance > (forward_core_heap.MinKey() + reverse_core_heap.MinKey()))
{ {
if (!forward_core_heap.Empty()) if (!forward_core_heap.Empty())
{ {
RoutingStep(forward_core_heap, reverse_core_heap, &middle, &distance, RoutingStep(forward_core_heap, reverse_core_heap, middle, distance,
min_edge_offset, true, false); min_core_edge_offset, true, false);
} }
if (!reverse_core_heap.Empty()) if (!reverse_core_heap.Empty())
{ {
RoutingStep(reverse_core_heap, forward_core_heap, &middle, &distance, RoutingStep(reverse_core_heap, forward_core_heap, middle, distance,
min_edge_offset, false, false); min_core_edge_offset, false, false);
} }
} }
// No path found for both target nodes? // No path found for both target nodes?
if (INVALID_EDGE_WEIGHT == distance) if (INVALID_EDGE_WEIGHT == distance || SPECIAL_NODEID == middle)
{ {
return; return;
} }
// Was a paths over one of the forward/reverse nodes not found? // 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"); "no path found");
// we need to unpack sub path from core heaps // we need to unpack sub path from core heaps
@ -685,12 +647,12 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
{ {
if (0 < forward_heap.Size()) 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); true);
} }
if (0 < reverse_heap.Size()) 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); false);
} }
} }
@ -704,7 +666,7 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
PhantomNodes nodes; PhantomNodes nodes;
nodes.source_phantom = source_phantom; nodes.source_phantom = source_phantom;
nodes.target_phantom = target_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 previous_coordinate = source_phantom.location;
FixedPointCoordinate current_coordinate; FixedPointCoordinate current_coordinate;

658
routing_algorithms/shortest_path.hpp
View File

@ -28,12 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef SHORTEST_PATH_HPP #ifndef SHORTEST_PATH_HPP
#define SHORTEST_PATH_HPP #define SHORTEST_PATH_HPP
#include <boost/assert.hpp> #include "../typedefs.h"
#include "routing_base.hpp" #include "routing_base.hpp"
#include "../data_structures/search_engine_data.hpp" #include "../data_structures/search_engine_data.hpp"
#include "../util/integer_range.hpp" #include "../util/integer_range.hpp"
#include "../typedefs.h"
#include <boost/assert.hpp>
template <class DataFacadeT> template <class DataFacadeT>
class ShortestPathRouting final class ShortestPathRouting final
@ -51,294 +53,482 @@ class ShortestPathRouting final
~ShortestPathRouting() {} ~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<NodeID> &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<NodeID> &leg_packed_path_forward,
std::vector<NodeID> &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<NodeID> &leg_packed_path_forward,
std::vector<NodeID> &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<PhantomNodes> &phantom_nodes_vector,
const std::vector<NodeID> &total_packed_path,
const std::vector<std::size_t> &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<std::size_t>(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<PhantomNodes> &phantom_nodes_vector, void operator()(const std::vector<PhantomNodes> &phantom_nodes_vector,
const std::vector<bool> &uturn_indicators, const std::vector<bool> &uturn_indicators,
InternalRouteResult &raw_route_data) const 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<std::vector<NodeID>> packed_legs1(phantom_nodes_vector.size());
std::vector<std::vector<NodeID>> packed_legs2(phantom_nodes_vector.size());
engine_working_data.InitializeOrClearFirstThreadLocalStorage( engine_working_data.InitializeOrClearFirstThreadLocalStorage(
super::facade->GetNumberOfNodes()); 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 &forward_heap = *(engine_working_data.forward_heap_1);
QueryHeap &reverse_heap1 = *(engine_working_data.reverse_heap_1); QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
QueryHeap &forward_heap2 = *(engine_working_data.forward_heap_2);
QueryHeap &reverse_heap2 = *(engine_working_data.reverse_heap_2); 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<NodeID> prev_packed_leg_to_forward;
std::vector<NodeID> prev_packed_leg_to_reverse;
std::vector<NodeID> total_packed_path_to_forward;
std::vector<std::size_t> packed_leg_to_forward_begin;
std::vector<NodeID> total_packed_path_to_reverse;
std::vector<std::size_t> packed_leg_to_reverse_begin;
std::size_t current_leg = 0; std::size_t current_leg = 0;
// Get distance to next pair of target nodes. // this implements a dynamic program that finds the shortest route through
for (const PhantomNodes &phantom_node_pair : phantom_nodes_vector) // a list of vias
for (const auto &phantom_node_pair : phantom_nodes_vector)
{ {
forward_heap1.Clear(); int new_total_distance_to_forward = INVALID_EDGE_WEIGHT;
forward_heap2.Clear(); int new_total_distance_to_reverse = INVALID_EDGE_WEIGHT;
reverse_heap1.Clear();
reverse_heap2.Clear();
int local_upper_bound1 = INVALID_EDGE_WEIGHT;
int local_upper_bound2 = INVALID_EDGE_WEIGHT;
middle1 = SPECIAL_NODEID; std::vector<NodeID> packed_leg_to_forward;
middle2 = SPECIAL_NODEID; std::vector<NodeID> packed_leg_to_reverse;
const bool allow_u_turn = current_leg > 0 && uturn_indicators.size() > current_leg && const auto &source_phantom = phantom_node_pair.source_phantom;
uturn_indicators[current_leg - 1]; const auto &target_phantom = phantom_node_pair.target_phantom;
const EdgeWeight min_edge_offset =
std::min(-phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(),
-phantom_node_pair.source_phantom.GetReverseWeightPlusOffset());
// insert new starting nodes into forward heap, adjusted by previous distances.
if ((allow_u_turn || search_from_1st_node) && const bool allow_u_turn_at_via = uturn_indicators.size() > current_leg &&
phantom_node_pair.source_phantom.forward_node_id != SPECIAL_NODEID) 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( search_to_forward_node = false;
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();
} }
if ((allow_u_turn || search_from_2nd_node) && if (source_phantom.reverse_node_id == target_phantom.reverse_node_id &&
phantom_node_pair.source_phantom.reverse_node_id != SPECIAL_NODEID) source_phantom.GetReverseWeightPlusOffset() > target_phantom.GetReverseWeightPlusOffset())
{ {
forward_heap1.Insert( search_to_reverse_node = false;
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();
} }
// insert new backward nodes into backward heap, unadjusted. BOOST_ASSERT(search_from_forward_node || search_from_reverse_node);
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();
}
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, if (allow_u_turn_at_via)
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()) 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_heap1, reverse_heap1, &middle1, &local_upper_bound1, new_total_distance_to_reverse = new_total_distance_to_forward;
min_edge_offset, true); packed_leg_to_reverse = packed_leg_to_forward;
}
if (!reverse_heap1.Empty())
{
super::RoutingStep(reverse_heap1, forward_heap1, &middle1, &local_upper_bound1,
min_edge_offset, false);
} }
} }
else
if (!reverse_heap2.Empty())
{ {
while (0 < (forward_heap2.Size() + reverse_heap2.Size())) Search(forward_heap, reverse_heap, search_from_forward_node,
{ search_from_reverse_node, search_to_forward_node, search_to_reverse_node,
if (!forward_heap2.Empty()) source_phantom, target_phantom, total_distance_to_forward,
{ total_distance_to_reverse, new_total_distance_to_forward,
super::RoutingStep(forward_heap2, reverse_heap2, &middle2, new_total_distance_to_reverse, packed_leg_to_forward, packed_leg_to_reverse);
&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);
} }
} }
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? // No path found for both target nodes?
if ((INVALID_EDGE_WEIGHT == local_upper_bound1) && if ((INVALID_EDGE_WEIGHT == new_total_distance_to_forward) &&
(INVALID_EDGE_WEIGHT == local_upper_bound2)) (INVALID_EDGE_WEIGHT == new_total_distance_to_reverse))
{ {
raw_route_data.shortest_path_length = INVALID_EDGE_WEIGHT; raw_route_data.shortest_path_length = INVALID_EDGE_WEIGHT;
raw_route_data.alternative_path_length = INVALID_EDGE_WEIGHT; raw_route_data.alternative_path_length = INVALID_EDGE_WEIGHT;
return; return;
} }
search_from_1st_node = true; // we need to figure out how the new legs connect to the previous ones
search_from_2nd_node = true; if (current_leg > 0)
if (SPECIAL_NODEID == middle1)
{ {
search_from_1st_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;
BOOST_ASSERT(!forward_to_forward || !reverse_to_forward);
BOOST_ASSERT(!forward_to_reverse || !reverse_to_reverse);
// in this case we always need to copy
if (forward_to_forward && forward_to_reverse)
{
// 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;
} }
if (SPECIAL_NODEID == middle2) else if (reverse_to_forward && reverse_to_reverse)
{ {
search_from_2nd_node = false; 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);
// Was at most one of the two paths not found? // in this case we just need to swap to regain the correct mapping
BOOST_ASSERT_MSG((INVALID_EDGE_WEIGHT != distance1 || INVALID_EDGE_WEIGHT != distance2), if (reverse_to_forward || forward_to_reverse)
"no path found");
// Unpack paths if they exist
std::vector<NodeID> temporary_packed_leg1;
std::vector<NodeID> 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, total_packed_path_to_forward.swap(total_packed_path_to_reverse);
temporary_packed_leg1); packed_leg_to_forward_begin.swap(packed_leg_to_reverse_begin);
}
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))
{
std::swap(temporary_packed_leg1, temporary_packed_leg2);
std::swap(local_upper_bound1, local_upper_bound2);
}
// remove the shorter path if both legs end at the same segment
if (start_id_of_leg1 == start_id_of_leg2)
{
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());
} }
} }
}
BOOST_ASSERT(packed_legs1.size() == packed_legs2.size());
packed_legs1[current_leg].insert(packed_legs1[current_leg].end(), if (new_total_distance_to_forward != INVALID_EDGE_WEIGHT)
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())
{ {
const NodeID last_node_id = packed_legs2[current_leg].back(); BOOST_ASSERT(target_phantom.forward_node_id != SPECIAL_NODEID);
search_from_1st_node &=
!(last_node_id == phantom_node_pair.target_phantom.reverse_node_id); packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size());
search_from_2nd_node &= total_packed_path_to_forward.insert(total_packed_path_to_forward.end(),
!(last_node_id == phantom_node_pair.target_phantom.forward_node_id); packed_leg_to_forward.begin(),
BOOST_ASSERT(search_from_1st_node != search_from_2nd_node); 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; if (new_total_distance_to_reverse != INVALID_EDGE_WEIGHT)
distance2 += local_upper_bound2; {
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; ++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); // insert sentinel
} packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size());
raw_route_data.unpacked_path_segments.resize(packed_legs1.size()); BOOST_ASSERT(packed_leg_to_reverse_begin.size() == phantom_nodes_vector.size() + 1);
for (const std::size_t index : osrm::irange<std::size_t>(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()); // insert sentinel
BOOST_ASSERT(packed_legs1.size() == raw_route_data.unpacked_path_segments.size()); 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]; UnpackLegs(phantom_nodes_vector, total_packed_path_to_forward,
super::UnpackPath( packed_leg_to_forward_begin, total_distance_to_forward, raw_route_data);
// -- 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));
} }
raw_route_data.shortest_path_length = std::min(distance1, distance2);
} }
}; };