InternalRouteResult directShortestPathSearch(
SearchEngineData<mld::Algorithm> &engine_working_data,
const datafacade::ContiguousInternalMemoryDataFacade<mld::Algorithm> &facade,
const PhantomNodes &phantom_nodes)
{
engine_working_data.InitializeOrClearFirstThreadLocalStorage(facade.GetNumberOfNodes());
auto &forward_heap = *engine_working_data.forward_heap_1;
auto &reverse_heap = *engine_working_data.reverse_heap_1;
insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);
// TODO: when structured bindings will be allowed change to
// auto [weight, source_node, target_node, unpacked_edges] = ...
EdgeWeight weight = INVALID_EDGE_WEIGHT;
std::vector<NodeID> unpacked_nodes;
std::vector<EdgeID> unpacked_edges;
std::tie(weight, unpacked_nodes, unpacked_edges) = mld::search(engine_working_data,
facade,
forward_heap,
reverse_heap,
DO_NOT_FORCE_LOOPS,
DO_NOT_FORCE_LOOPS,
INVALID_EDGE_WEIGHT,
phantom_nodes);
return extractRoute(facade, weight, phantom_nodes, unpacked_nodes, unpacked_edges);
}
InternalRouteResult
directShortestPathSearch(SearchEngineData<Algorithm> &engine_working_data,
const datafacade::ContiguousInternalMemoryDataFacade<Algorithm> &facade,
const PhantomNodes &phantom_nodes)
{
engine_working_data.InitializeOrClearFirstThreadLocalStorage(facade.GetNumberOfNodes());
auto &forward_heap = *engine_working_data.forward_heap_1;
auto &reverse_heap = *engine_working_data.reverse_heap_1;
forward_heap.Clear();
reverse_heap.Clear();
EdgeWeight weight = INVALID_EDGE_WEIGHT;
std::vector<NodeID> packed_leg;
insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);
search(engine_working_data,
facade,
forward_heap,
reverse_heap,
weight,
packed_leg,
DO_NOT_FORCE_LOOPS,
DO_NOT_FORCE_LOOPS,
phantom_nodes);
std::vector<NodeID> unpacked_nodes;
std::vector<EdgeID> unpacked_edges;
if (!packed_leg.empty())
{
unpacked_nodes.reserve(packed_leg.size());
unpacked_edges.reserve(packed_leg.size());
unpacked_nodes.push_back(packed_leg.front());
ch::unpackPath(facade,
packed_leg.begin(),
packed_leg.end(),
[&unpacked_nodes, &unpacked_edges](std::pair<NodeID, NodeID> &edge,
const auto &edge_id) {
BOOST_ASSERT(edge.first == unpacked_nodes.back());
unpacked_nodes.push_back(edge.second);
unpacked_edges.push_back(edge_id);
});
}
return extractRoute(facade, weight, phantom_nodes, unpacked_nodes, unpacked_edges);
}
std::vector<TurnData>
getTileTurns(const datafacade::ContiguousInternalMemoryDataFacade<algorithm::CH> &facade,
const std::vector<RTreeLeaf> &edges,
const std::vector<std::size_t> &sorted_edge_indexes)
{
std::vector<TurnData> all_turn_data;
// Struct to hold info on all the EdgeBasedNodes that are visible in our tile
// When we create these, we insure that (source, target) and packed_geometry_id
// are all pointed in the same direction.
struct EdgeBasedNodeInfo
{
bool is_geometry_forward; // Is the geometry forward or reverse?
unsigned packed_geometry_id;
};
// Lookup table for edge-based-nodes
std::unordered_map<NodeID, EdgeBasedNodeInfo> edge_based_node_info;
struct SegmentData
{
NodeID target_node;
EdgeID edge_based_node_id;
};
std::unordered_map<NodeID, std::vector<SegmentData>> directed_graph;
// Reserve enough space for unique edge-based-nodes on every edge.
// Only a tile with all unique edges will use this much, but
// it saves us a bunch of re-allocations during iteration.
directed_graph.reserve(edges.size() * 2);
// Build an adjacency list for all the road segments visible in
// the tile
for (const auto &edge_index : sorted_edge_indexes)
{
const auto &edge = edges[edge_index];
if (edge.forward_segment_id.enabled)
{
// operator[] will construct an empty vector at [edge.u] if there is no value.
directed_graph[edge.u].push_back({edge.v, edge.forward_segment_id.id});
if (edge_based_node_info.count(edge.forward_segment_id.id) == 0)
{
edge_based_node_info[edge.forward_segment_id.id] = {true, edge.packed_geometry_id};
}
else
{
BOOST_ASSERT(edge_based_node_info[edge.forward_segment_id.id].is_geometry_forward ==
true);
BOOST_ASSERT(edge_based_node_info[edge.forward_segment_id.id].packed_geometry_id ==
edge.packed_geometry_id);
}
}
if (edge.reverse_segment_id.enabled)
{
directed_graph[edge.v].push_back({edge.u, edge.reverse_segment_id.id});
if (edge_based_node_info.count(edge.reverse_segment_id.id) == 0)
{
edge_based_node_info[edge.reverse_segment_id.id] = {false, edge.packed_geometry_id};
}
else
{
BOOST_ASSERT(edge_based_node_info[edge.reverse_segment_id.id].is_geometry_forward ==
false);
BOOST_ASSERT(edge_based_node_info[edge.reverse_segment_id.id].packed_geometry_id ==
edge.packed_geometry_id);
}
}
}
// Given a turn:
// u---v
// |
// w
// uv is the "approach"
// vw is the "exit"
std::vector<contractor::QueryEdge::EdgeData> unpacked_shortcut;
std::vector<EdgeWeight> approach_weight_vector;
std::vector<EdgeWeight> approach_duration_vector;
// Make sure we traverse the startnodes in a consistent order
// to ensure identical PBF encoding on all platforms.
std::vector<NodeID> sorted_startnodes;
sorted_startnodes.reserve(directed_graph.size());
for (const auto &startnode : directed_graph)
sorted_startnodes.push_back(startnode.first);
std::sort(sorted_startnodes.begin(), sorted_startnodes.end());
// Look at every node in the directed graph we created
for (const auto &startnode : sorted_startnodes)
{
const auto &nodedata = directed_graph[startnode];
// For all the outgoing edges from the node
for (const auto &approachedge : nodedata)
{
// If the target of this edge doesn't exist in our directed
// graph, it's probably outside the tile, so we can skip it
if (directed_graph.count(approachedge.target_node) == 0)
continue;
// For each of the outgoing edges from our target coordinate
for (const auto &exit_edge : directed_graph[approachedge.target_node])
{
// If the next edge has the same edge_based_node_id, then it's
// not a turn, so skip it
if (approachedge.edge_based_node_id == exit_edge.edge_based_node_id)
continue;
// Skip u-turns
if (startnode == exit_edge.target_node)
continue;
// Find the connection between our source road and the target node
// Since we only want to find direct edges, we cannot check shortcut edges here.
// Otherwise we might find a forward edge even though a shorter backward edge
// exists (due to oneways).
//
// a > - > - > - b
// | |
// |------ c ----|
//
// would offer a backward edge at `b` to `a` (due to the oneway from a to b)
// but could also offer a shortcut (b-c-a) from `b` to `a` which is longer.
EdgeID smaller_edge_id =
facade.FindSmallestEdge(approachedge.edge_based_node_id,
exit_edge.edge_based_node_id,
[](const contractor::QueryEdge::EdgeData &data) {
return data.forward && !data.shortcut;
});
// Depending on how the graph is constructed, we might have to look for
// a backwards edge instead. They're equivalent, just one is available for
// a forward routing search, and one is used for the backwards dijkstra
// steps. Their weight should be the same, we can use either one.
// If we didn't find a forward edge, try for a backward one
if (SPECIAL_EDGEID == smaller_edge_id)
{
smaller_edge_id =
facade.FindSmallestEdge(exit_edge.edge_based_node_id,
approachedge.edge_based_node_id,
[](const contractor::QueryEdge::EdgeData &data) {
return data.backward && !data.shortcut;
});
}
// If no edge was found, it means that there's no connection between these
// nodes, due to oneways or turn restrictions. Given the edge-based-nodes
// that we're examining here, we *should* only find directly-connected
// edges, not shortcuts
if (smaller_edge_id != SPECIAL_EDGEID)
{
const auto &data = facade.GetEdgeData(smaller_edge_id);
BOOST_ASSERT_MSG(!data.shortcut, "Connecting edge must not be a shortcut");
// Now, calculate the sum of the weight of all the segments.
if (edge_based_node_info[approachedge.edge_based_node_id].is_geometry_forward)
{
approach_weight_vector = facade.GetUncompressedForwardWeights(
edge_based_node_info[approachedge.edge_based_node_id]
.packed_geometry_id);
approach_duration_vector = facade.GetUncompressedForwardDurations(
edge_based_node_info[approachedge.edge_based_node_id]
.packed_geometry_id);
}
else
{
approach_weight_vector = facade.GetUncompressedReverseWeights(
edge_based_node_info[approachedge.edge_based_node_id]
.packed_geometry_id);
approach_duration_vector = facade.GetUncompressedReverseDurations(
edge_based_node_info[approachedge.edge_based_node_id]
.packed_geometry_id);
}
const auto sum_node_weight = std::accumulate(approach_weight_vector.begin(),
approach_weight_vector.end(),
EdgeWeight{0});
const auto sum_node_duration = std::accumulate(approach_duration_vector.begin(),
approach_duration_vector.end(),
EdgeWeight{0});
// The edge.weight is the whole edge weight, which includes the turn
// cost.
// The turn cost is the edge.weight minus the sum of the individual road
// segment weights. This might not be 100% accurate, because some
// intersections include stop signs, traffic signals and other
// penalties, but at this stage, we can't divide those out, so we just
// treat the whole lot as the "turn cost" that we'll stick on the map.
const auto turn_weight = data.weight - sum_node_weight;
const auto turn_duration = data.duration - sum_node_duration;
// Find the three nodes that make up the turn movement)
const auto node_from = startnode;
const auto node_via = approachedge.target_node;
const auto node_to = exit_edge.target_node;
const auto coord_from = facade.GetCoordinateOfNode(node_from);
const auto coord_via = facade.GetCoordinateOfNode(node_via);
const auto coord_to = facade.GetCoordinateOfNode(node_to);
// Calculate the bearing that we approach the intersection at
const auto angle_in = static_cast<int>(
util::coordinate_calculation::bearing(coord_from, coord_via));
const auto exit_bearing = static_cast<int>(
util::coordinate_calculation::bearing(coord_via, coord_to));
// Figure out the angle of the turn
auto turn_angle = exit_bearing - angle_in;
while (turn_angle > 180)
{
turn_angle -= 360;
}
while (turn_angle < -180)
{
turn_angle += 360;
}
// Save everything we need to later add all the points to the tile.
// We need the coordinate of the intersection, the angle in, the turn
// angle and the turn cost.
all_turn_data.push_back(
TurnData{coord_via, angle_in, turn_angle, turn_weight, turn_duration});
}
}
}
}
return all_turn_data;
}
InternalRouteResult
shortestPathSearch(SearchEngineData<Algorithm> &engine_working_data,
const datafacade::ContiguousInternalMemoryDataFacade<Algorithm> &facade,
const std::vector<PhantomNodes> &phantom_nodes_vector,
const boost::optional<bool> continue_straight_at_waypoint)
{
InternalRouteResult raw_route_data;
raw_route_data.segment_end_coordinates = phantom_nodes_vector;
const bool allow_uturn_at_waypoint =
!(continue_straight_at_waypoint ? *continue_straight_at_waypoint
: facade.GetContinueStraightDefault());
engine_working_data.InitializeOrClearFirstThreadLocalStorage(facade.GetNumberOfNodes());
auto &forward_heap = *engine_working_data.forward_heap_1;
auto &reverse_heap = *engine_working_data.reverse_heap_1;
int total_weight_to_forward = 0;
int total_weight_to_reverse = 0;
bool search_from_forward_node =
phantom_nodes_vector.front().source_phantom.IsValidForwardSource();
bool search_from_reverse_node =
phantom_nodes_vector.front().source_phantom.IsValidReverseSource();
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;
// this implements a dynamic program that finds the shortest route through
// a list of vias
for (const auto &phantom_node_pair : phantom_nodes_vector)
{
int new_total_weight_to_forward = INVALID_EDGE_WEIGHT;
int new_total_weight_to_reverse = INVALID_EDGE_WEIGHT;
std::vector<NodeID> packed_leg_to_forward;
std::vector<NodeID> packed_leg_to_reverse;
const auto &source_phantom = phantom_node_pair.source_phantom;
const auto &target_phantom = phantom_node_pair.target_phantom;
bool search_to_forward_node = target_phantom.IsValidForwardTarget();
bool search_to_reverse_node = target_phantom.IsValidReverseTarget();
BOOST_ASSERT(!search_from_forward_node || source_phantom.IsValidForwardSource());
BOOST_ASSERT(!search_from_reverse_node || source_phantom.IsValidReverseSource());
if (search_to_reverse_node || search_to_forward_node)
{
if (allow_uturn_at_waypoint)
{
searchWithUTurn(engine_working_data,
facade,
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_weight_to_forward,
total_weight_to_reverse,
new_total_weight_to_forward,
packed_leg_to_forward);
// if only the reverse node is valid (e.g. when using the match plugin) we
// actually need to move
if (!target_phantom.IsValidForwardTarget())
{
BOOST_ASSERT(target_phantom.IsValidReverseTarget());
new_total_weight_to_reverse = new_total_weight_to_forward;
packed_leg_to_reverse = std::move(packed_leg_to_forward);
new_total_weight_to_forward = INVALID_EDGE_WEIGHT;
// (*)
//
// Below we have to check if new_total_weight_to_forward is invalid.
// This prevents use-after-move on packed_leg_to_forward.
}
else if (target_phantom.IsValidReverseTarget())
{
new_total_weight_to_reverse = new_total_weight_to_forward;
packed_leg_to_reverse = packed_leg_to_forward;
}
}
else
{
search(engine_working_data,
facade,
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_weight_to_forward,
total_weight_to_reverse,
new_total_weight_to_forward,
new_total_weight_to_reverse,
packed_leg_to_forward,
packed_leg_to_reverse);
}
}
// Note: To make sure we do not access the moved-from packed_leg_to_forward
// we guard its access by a check for invalid edge weight. See (*) above.
// No path found for both target nodes?
if ((INVALID_EDGE_WEIGHT == new_total_weight_to_forward) &&
(INVALID_EDGE_WEIGHT == new_total_weight_to_reverse))
{
return raw_route_data;
}
// we need to figure out how the new legs connect to the previous ones
if (current_leg > 0)
{
bool forward_to_forward =
(new_total_weight_to_forward != INVALID_EDGE_WEIGHT) &&
packed_leg_to_forward.front() == source_phantom.forward_segment_id.id;
bool reverse_to_forward =
(new_total_weight_to_forward != INVALID_EDGE_WEIGHT) &&
packed_leg_to_forward.front() == source_phantom.reverse_segment_id.id;
bool forward_to_reverse =
(new_total_weight_to_reverse != INVALID_EDGE_WEIGHT) &&
packed_leg_to_reverse.front() == source_phantom.forward_segment_id.id;
bool reverse_to_reverse =
(new_total_weight_to_reverse != INVALID_EDGE_WEIGHT) &&
packed_leg_to_reverse.front() == source_phantom.reverse_segment_id.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;
}
else if (reverse_to_forward && reverse_to_reverse)
{
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);
}
}
if (new_total_weight_to_forward != INVALID_EDGE_WEIGHT)
{
BOOST_ASSERT(target_phantom.IsValidForwardTarget());
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;
}
if (new_total_weight_to_reverse != INVALID_EDGE_WEIGHT)
{
BOOST_ASSERT(target_phantom.IsValidReverseTarget());
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_weight_to_forward = new_total_weight_to_forward;
total_weight_to_reverse = new_total_weight_to_reverse;
++current_leg;
}
BOOST_ASSERT(total_weight_to_forward != INVALID_EDGE_WEIGHT ||
total_weight_to_reverse != INVALID_EDGE_WEIGHT);
// We make sure the fastest route is always in packed_legs_to_forward
if (total_weight_to_forward < total_weight_to_reverse ||
(total_weight_to_forward == total_weight_to_reverse &&
total_packed_path_to_forward.size() < total_packed_path_to_reverse.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);
unpackLegs(facade,
phantom_nodes_vector,
total_packed_path_to_forward,
packed_leg_to_forward_begin,
total_weight_to_forward,
raw_route_data);
}
else
{
// 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);
unpackLegs(facade,
phantom_nodes_vector,
total_packed_path_to_reverse,
packed_leg_to_reverse_begin,
total_weight_to_reverse,
raw_route_data);
}
return raw_route_data;
}
std::vector<EdgeWeight>
manyToManySearch(SearchEngineData &engine_working_data,
const datafacade::ContiguousInternalMemoryDataFacade<algorithm::CH> &facade,
const std::vector<PhantomNode> &phantom_nodes,
const std::vector<std::size_t> &source_indices,
const std::vector<std::size_t> &target_indices)
{
const auto number_of_sources =
source_indices.empty() ? phantom_nodes.size() : source_indices.size();
const auto number_of_targets =
target_indices.empty() ? phantom_nodes.size() : target_indices.size();
const auto number_of_entries = number_of_sources * number_of_targets;
std::vector<EdgeWeight> weights_table(number_of_entries, INVALID_EDGE_WEIGHT);
std::vector<EdgeWeight> durations_table(number_of_entries, MAXIMAL_EDGE_DURATION);
engine_working_data.InitializeOrClearManyToManyThreadLocalStorage(facade.GetNumberOfNodes());
auto &query_heap = *(engine_working_data.many_to_many_heap);
SearchSpaceWithBuckets search_space_with_buckets;
unsigned column_idx = 0;
const auto search_target_phantom = [&](const PhantomNode &phantom) {
// clear heap and insert target nodes
query_heap.Clear();
insertNodesInHeap<REVERSE_DIRECTION>(query_heap, phantom);
// explore search space
while (!query_heap.Empty())
{
backwardRoutingStep(facade, column_idx, query_heap, search_space_with_buckets);
}
++column_idx;
};
// for each source do forward search
unsigned row_idx = 0;
const auto search_source_phantom = [&](const PhantomNode &phantom) {
// clear heap and insert source nodes
query_heap.Clear();
insertNodesInHeap<FORWARD_DIRECTION>(query_heap, phantom);
// explore search space
while (!query_heap.Empty())
{
forwardRoutingStep(facade,
row_idx,
number_of_targets,
query_heap,
search_space_with_buckets,
weights_table,
durations_table);
}
++row_idx;
};
if (target_indices.empty())
{
for (const auto &phantom : phantom_nodes)
{
search_target_phantom(phantom);
}
}
else
{
for (const auto index : target_indices)
{
const auto &phantom = phantom_nodes[index];
search_target_phantom(phantom);
}
}
if (source_indices.empty())
{
for (const auto &phantom : phantom_nodes)
{
search_source_phantom(phantom);
}
}
else
{
for (const auto index : source_indices)
{
const auto &phantom = phantom_nodes[index];
search_source_phantom(phantom);
}
}
return durations_table;
}
