769 lines
29 KiB
C++
769 lines
29 KiB
C++
#ifndef OSRM_ENGINE_ROUTING_BASE_MLD_HPP
|
|
#define OSRM_ENGINE_ROUTING_BASE_MLD_HPP
|
|
|
|
#include "engine/algorithm.hpp"
|
|
#include "engine/datafacade.hpp"
|
|
#include "engine/routing_algorithms/routing_base.hpp"
|
|
#include "engine/search_engine_data.hpp"
|
|
|
|
#include "util/typedefs.hpp"
|
|
|
|
#include <boost/assert.hpp>
|
|
|
|
#include <algorithm>
|
|
#include <boost/core/ignore_unused.hpp>
|
|
#include <iterator>
|
|
#include <limits>
|
|
#include <tuple>
|
|
#include <vector>
|
|
|
|
namespace osrm::engine::routing_algorithms::mld
|
|
{
|
|
|
|
namespace
|
|
{
|
|
// Unrestricted search (Args is const PhantomNodes &):
|
|
// * use partition.GetQueryLevel to find the node query level based on source and target phantoms
|
|
// * allow to traverse all cells
|
|
template <typename MultiLevelPartition>
|
|
inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
|
|
NodeID node,
|
|
const PhantomNode &source,
|
|
const PhantomNode &target)
|
|
{
|
|
auto level = [&partition, node](const SegmentID &source, const SegmentID &target)
|
|
{
|
|
if (source.enabled && target.enabled)
|
|
return partition.GetQueryLevel(source.id, target.id, node);
|
|
return INVALID_LEVEL_ID;
|
|
};
|
|
|
|
return std::min(std::min(level(source.forward_segment_id, target.forward_segment_id),
|
|
level(source.forward_segment_id, target.reverse_segment_id)),
|
|
std::min(level(source.reverse_segment_id, target.forward_segment_id),
|
|
level(source.reverse_segment_id, target.reverse_segment_id)));
|
|
}
|
|
|
|
template <typename MultiLevelPartition>
|
|
inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
|
|
NodeID node,
|
|
const PhantomEndpoints &endpoints)
|
|
{
|
|
return getNodeQueryLevel(partition, node, endpoints.source_phantom, endpoints.target_phantom);
|
|
}
|
|
|
|
template <typename MultiLevelPartition>
|
|
inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
|
|
NodeID node,
|
|
const PhantomCandidatesToTarget &endpoint_candidates)
|
|
{
|
|
auto min_level = std::accumulate(
|
|
endpoint_candidates.source_phantoms.begin(),
|
|
endpoint_candidates.source_phantoms.end(),
|
|
INVALID_LEVEL_ID,
|
|
[&](LevelID current_level, const PhantomNode &source)
|
|
{
|
|
return std::min(
|
|
current_level,
|
|
getNodeQueryLevel(partition, node, source, endpoint_candidates.target_phantom));
|
|
});
|
|
return min_level;
|
|
}
|
|
|
|
template <typename MultiLevelPartition>
|
|
inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
|
|
NodeID node,
|
|
const PhantomEndpointCandidates &endpoint_candidates)
|
|
{
|
|
auto min_level = std::accumulate(
|
|
endpoint_candidates.source_phantoms.begin(),
|
|
endpoint_candidates.source_phantoms.end(),
|
|
INVALID_LEVEL_ID,
|
|
[&](LevelID level_1, const PhantomNode &source)
|
|
{
|
|
return std::min(
|
|
level_1,
|
|
std::accumulate(endpoint_candidates.target_phantoms.begin(),
|
|
endpoint_candidates.target_phantoms.end(),
|
|
level_1,
|
|
[&](LevelID level_2, const PhantomNode &target) {
|
|
return std::min(
|
|
level_2,
|
|
getNodeQueryLevel(partition, node, source, target));
|
|
}));
|
|
});
|
|
return min_level;
|
|
}
|
|
|
|
template <typename PhantomCandidateT>
|
|
inline bool checkParentCellRestriction(CellID, const PhantomCandidateT &)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// Restricted search (Args is LevelID, CellID):
|
|
// * use the fixed level for queries
|
|
// * check if the node cell is the same as the specified parent
|
|
template <typename MultiLevelPartition>
|
|
inline LevelID getNodeQueryLevel(const MultiLevelPartition &, NodeID, LevelID level, CellID)
|
|
{
|
|
return level;
|
|
}
|
|
|
|
inline bool checkParentCellRestriction(CellID cell, LevelID, CellID parent)
|
|
{
|
|
return cell == parent;
|
|
}
|
|
|
|
// Unrestricted search with a single phantom node (Args is const PhantomNode &):
|
|
// * use partition.GetQueryLevel to find the node query level
|
|
// * allow to traverse all cells
|
|
template <typename MultiLevelPartition>
|
|
inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
|
|
const NodeID node,
|
|
const PhantomNodeCandidates &candidates)
|
|
{
|
|
auto highest_different_level = [&partition, node](const SegmentID &segment)
|
|
{
|
|
return segment.enabled ? partition.GetHighestDifferentLevel(segment.id, node)
|
|
: INVALID_LEVEL_ID;
|
|
};
|
|
|
|
auto node_level =
|
|
std::accumulate(candidates.begin(),
|
|
candidates.end(),
|
|
INVALID_LEVEL_ID,
|
|
[&](LevelID current_level, const PhantomNode &phantom_node)
|
|
{
|
|
auto highest_level =
|
|
std::min(highest_different_level(phantom_node.forward_segment_id),
|
|
highest_different_level(phantom_node.reverse_segment_id));
|
|
return std::min(current_level, highest_level);
|
|
});
|
|
return node_level;
|
|
}
|
|
|
|
// Unrestricted search with a single phantom node and a vector of phantom nodes:
|
|
// * use partition.GetQueryLevel to find the node query level
|
|
// * allow to traverse all cells
|
|
template <typename MultiLevelPartition>
|
|
inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
|
|
NodeID node,
|
|
const std::vector<PhantomNodeCandidates> &candidates_list,
|
|
const std::size_t phantom_index,
|
|
const std::vector<std::size_t> &phantom_indices)
|
|
{
|
|
// Get minimum level over all phantoms of the highest different level with respect to node
|
|
// This is equivalent to min_{∀ source, target} partition.GetQueryLevel(source, node, target)
|
|
auto init = getNodeQueryLevel(partition, node, candidates_list[phantom_index]);
|
|
auto result = std::accumulate(
|
|
phantom_indices.begin(),
|
|
phantom_indices.end(),
|
|
init,
|
|
[&](LevelID level, size_t index)
|
|
{ return std::min(level, getNodeQueryLevel(partition, node, candidates_list[index])); });
|
|
return result;
|
|
}
|
|
} // namespace
|
|
|
|
// Heaps only record for each node its predecessor ("parent") on the shortest path.
|
|
// For re-constructing the actual path we need to trace back all parent "pointers".
|
|
// In contrast to the CH code MLD needs to know the edges (with clique arc property).
|
|
|
|
using PackedEdge = std::tuple</*from*/ NodeID, /*to*/ NodeID, /*from_clique_arc*/ bool>;
|
|
using PackedPath = std::vector<PackedEdge>;
|
|
|
|
template <bool DIRECTION, typename OutIter>
|
|
inline void retrievePackedPathFromSingleManyToManyHeap(
|
|
const SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap, const NodeID middle, OutIter out)
|
|
{
|
|
|
|
NodeID current = middle;
|
|
NodeID parent = heap.GetData(current).parent;
|
|
|
|
while (current != parent)
|
|
{
|
|
const auto &data = heap.GetData(current);
|
|
|
|
if (DIRECTION == FORWARD_DIRECTION)
|
|
{
|
|
*out = std::make_tuple(parent, current, data.from_clique_arc);
|
|
++out;
|
|
}
|
|
else if (DIRECTION == REVERSE_DIRECTION)
|
|
{
|
|
*out = std::make_tuple(current, parent, data.from_clique_arc);
|
|
++out;
|
|
}
|
|
|
|
current = parent;
|
|
parent = heap.GetData(parent).parent;
|
|
}
|
|
}
|
|
|
|
template <bool DIRECTION>
|
|
inline PackedPath retrievePackedPathFromSingleManyToManyHeap(
|
|
const SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap, const NodeID middle)
|
|
{
|
|
|
|
PackedPath packed_path;
|
|
retrievePackedPathFromSingleManyToManyHeap<DIRECTION>(
|
|
heap, middle, std::back_inserter(packed_path));
|
|
|
|
return packed_path;
|
|
}
|
|
|
|
template <bool DIRECTION, typename OutIter>
|
|
inline void retrievePackedPathFromSingleHeap(const SearchEngineData<Algorithm>::QueryHeap &heap,
|
|
const NodeID middle,
|
|
OutIter out)
|
|
{
|
|
NodeID current = middle;
|
|
NodeID parent = heap.GetData(current).parent;
|
|
|
|
while (current != parent)
|
|
{
|
|
const auto &data = heap.GetData(current);
|
|
|
|
if (DIRECTION == FORWARD_DIRECTION)
|
|
{
|
|
*out = std::make_tuple(parent, current, data.from_clique_arc);
|
|
++out;
|
|
}
|
|
else if (DIRECTION == REVERSE_DIRECTION)
|
|
{
|
|
*out = std::make_tuple(current, parent, data.from_clique_arc);
|
|
++out;
|
|
}
|
|
|
|
current = parent;
|
|
parent = heap.GetData(parent).parent;
|
|
}
|
|
}
|
|
|
|
template <bool DIRECTION>
|
|
inline PackedPath
|
|
retrievePackedPathFromSingleHeap(const SearchEngineData<Algorithm>::QueryHeap &heap,
|
|
const NodeID middle)
|
|
{
|
|
PackedPath packed_path;
|
|
retrievePackedPathFromSingleHeap<DIRECTION>(heap, middle, std::back_inserter(packed_path));
|
|
return packed_path;
|
|
}
|
|
|
|
// Trace path from middle to start in the forward search space (in reverse)
|
|
// and from middle to end in the reverse search space. Middle connects paths.
|
|
|
|
inline PackedPath
|
|
retrievePackedPathFromHeap(const SearchEngineData<Algorithm>::QueryHeap &forward_heap,
|
|
const SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
|
|
const NodeID middle)
|
|
{
|
|
// Retrieve start -> middle. Is in reverse order since tracing back starts from middle.
|
|
auto packed_path = retrievePackedPathFromSingleHeap<FORWARD_DIRECTION>(forward_heap, middle);
|
|
std::reverse(begin(packed_path), end(packed_path));
|
|
|
|
// Retrieve middle -> end. Is already in correct order, tracing starts from middle.
|
|
auto into = std::back_inserter(packed_path);
|
|
retrievePackedPathFromSingleHeap<REVERSE_DIRECTION>(reverse_heap, middle, into);
|
|
|
|
return packed_path;
|
|
}
|
|
|
|
template <typename Heap>
|
|
void insertOrUpdate(Heap &heap,
|
|
const NodeID node,
|
|
const EdgeWeight weight,
|
|
const typename Heap::DataType &data)
|
|
{
|
|
const auto heapNode = heap.GetHeapNodeIfWasInserted(node);
|
|
if (!heapNode)
|
|
{
|
|
heap.Insert(node, weight, data);
|
|
}
|
|
else if (weight < heapNode->weight)
|
|
{
|
|
heapNode->data = data;
|
|
heapNode->weight = weight;
|
|
heap.DecreaseKey(*heapNode);
|
|
}
|
|
}
|
|
|
|
template <bool DIRECTION, typename Algorithm, typename Heap, typename... Args>
|
|
void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
|
|
Heap &forward_heap,
|
|
const typename Heap::HeapNode &heapNode,
|
|
const Args &...args)
|
|
{
|
|
const auto &partition = facade.GetMultiLevelPartition();
|
|
const auto &cells = facade.GetCellStorage();
|
|
const auto &metric = facade.GetCellMetric();
|
|
|
|
const auto level = getNodeQueryLevel(partition, heapNode.node, args...);
|
|
|
|
static constexpr auto IS_MAP_MATCHING =
|
|
std::is_same_v<typename SearchEngineData<mld::Algorithm>::MapMatchingQueryHeap, Heap>;
|
|
|
|
if (level >= 1 && !heapNode.data.from_clique_arc)
|
|
{
|
|
if constexpr (DIRECTION == FORWARD_DIRECTION)
|
|
{
|
|
// Shortcuts in forward direction
|
|
const auto &cell =
|
|
cells.GetCell(metric, level, partition.GetCell(level, heapNode.node));
|
|
auto destination = cell.GetDestinationNodes().begin();
|
|
auto distance = [&cell, node = heapNode.node ]() -> auto
|
|
{
|
|
if constexpr (IS_MAP_MATCHING)
|
|
{
|
|
|
|
return cell.GetOutDistance(node).begin();
|
|
}
|
|
else
|
|
{
|
|
boost::ignore_unused(cell, node);
|
|
return 0;
|
|
}
|
|
}
|
|
();
|
|
for (auto shortcut_weight : cell.GetOutWeight(heapNode.node))
|
|
{
|
|
BOOST_ASSERT(destination != cell.GetDestinationNodes().end());
|
|
const NodeID to = *destination;
|
|
|
|
if (shortcut_weight != INVALID_EDGE_WEIGHT && heapNode.node != to)
|
|
{
|
|
const EdgeWeight to_weight = heapNode.weight + shortcut_weight;
|
|
BOOST_ASSERT(to_weight >= heapNode.weight);
|
|
|
|
if constexpr (IS_MAP_MATCHING)
|
|
{
|
|
const EdgeDistance to_distance = heapNode.data.distance + *distance;
|
|
insertOrUpdate(
|
|
forward_heap, to, to_weight, {heapNode.node, true, to_distance});
|
|
}
|
|
else
|
|
{
|
|
insertOrUpdate(forward_heap, to, to_weight, {heapNode.node, true});
|
|
}
|
|
}
|
|
++destination;
|
|
if constexpr (IS_MAP_MATCHING)
|
|
{
|
|
++distance;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Shortcuts in backward direction
|
|
const auto &cell =
|
|
cells.GetCell(metric, level, partition.GetCell(level, heapNode.node));
|
|
auto source = cell.GetSourceNodes().begin();
|
|
auto distance = [&cell, node = heapNode.node ]() -> auto
|
|
{
|
|
if constexpr (IS_MAP_MATCHING)
|
|
{
|
|
|
|
return cell.GetInDistance(node).begin();
|
|
}
|
|
else
|
|
{
|
|
boost::ignore_unused(cell, node);
|
|
return 0;
|
|
}
|
|
}
|
|
();
|
|
for (auto shortcut_weight : cell.GetInWeight(heapNode.node))
|
|
{
|
|
BOOST_ASSERT(source != cell.GetSourceNodes().end());
|
|
const NodeID to = *source;
|
|
|
|
if (shortcut_weight != INVALID_EDGE_WEIGHT && heapNode.node != to)
|
|
{
|
|
const EdgeWeight to_weight = heapNode.weight + shortcut_weight;
|
|
BOOST_ASSERT(to_weight >= heapNode.weight);
|
|
if constexpr (IS_MAP_MATCHING)
|
|
{
|
|
const EdgeDistance to_distance = heapNode.data.distance + *distance;
|
|
insertOrUpdate(
|
|
forward_heap, to, to_weight, {heapNode.node, true, to_distance});
|
|
}
|
|
else
|
|
{
|
|
insertOrUpdate(forward_heap, to, to_weight, {heapNode.node, true});
|
|
}
|
|
}
|
|
++source;
|
|
if constexpr (IS_MAP_MATCHING)
|
|
{
|
|
++distance;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Boundary edges
|
|
for (const auto edge : facade.GetBorderEdgeRange(level, heapNode.node))
|
|
{
|
|
const auto &edge_data = facade.GetEdgeData(edge);
|
|
|
|
if ((DIRECTION == FORWARD_DIRECTION) ? facade.IsForwardEdge(edge)
|
|
: facade.IsBackwardEdge(edge))
|
|
{
|
|
const NodeID to = facade.GetTarget(edge);
|
|
|
|
if (!facade.ExcludeNode(to) &&
|
|
checkParentCellRestriction(partition.GetCell(level + 1, to), args...))
|
|
{
|
|
const auto node_weight =
|
|
facade.GetNodeWeight(DIRECTION == FORWARD_DIRECTION ? heapNode.node : to);
|
|
const auto turn_penalty = facade.GetWeightPenaltyForEdgeID(edge_data.turn_id);
|
|
|
|
// TODO: BOOST_ASSERT(edge_data.weight == node_weight + turn_penalty);
|
|
|
|
const EdgeWeight to_weight =
|
|
heapNode.weight + node_weight + alias_cast<EdgeWeight>(turn_penalty);
|
|
|
|
if constexpr (IS_MAP_MATCHING)
|
|
{
|
|
const auto node_distance =
|
|
facade.GetNodeDistance(DIRECTION == FORWARD_DIRECTION ? heapNode.node : to);
|
|
|
|
const EdgeDistance to_distance = heapNode.data.distance + node_distance;
|
|
insertOrUpdate(
|
|
forward_heap, to, to_weight, {heapNode.node, false, to_distance});
|
|
}
|
|
else
|
|
{
|
|
insertOrUpdate(forward_heap, to, to_weight, {heapNode.node, false});
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
template <bool DIRECTION, typename Algorithm, typename Heap, typename... Args>
|
|
void routingStep(const DataFacade<Algorithm> &facade,
|
|
Heap &forward_heap,
|
|
Heap &reverse_heap,
|
|
NodeID &middle_node,
|
|
EdgeWeight &path_upper_bound,
|
|
const std::vector<NodeID> &force_step_nodes,
|
|
const Args &...args)
|
|
{
|
|
const auto heapNode = forward_heap.DeleteMinGetHeapNode();
|
|
const auto weight = heapNode.weight;
|
|
|
|
BOOST_ASSERT(!facade.ExcludeNode(heapNode.node));
|
|
|
|
// Upper bound for the path source -> target with
|
|
// weight(source -> node) = weight weight(to -> target) ≤ reverse_weight
|
|
// is weight + reverse_weight
|
|
// More tighter upper bound requires additional condition reverse_heap.WasRemoved(to)
|
|
// with weight(to -> target) = reverse_weight and all weights ≥ 0
|
|
const auto reverseHeapNode = reverse_heap.GetHeapNodeIfWasInserted(heapNode.node);
|
|
if (reverseHeapNode)
|
|
{
|
|
auto reverse_weight = reverseHeapNode->weight;
|
|
auto path_weight = weight + reverse_weight;
|
|
|
|
if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) &&
|
|
(path_weight >= EdgeWeight{0}) && (path_weight < path_upper_bound))
|
|
{
|
|
middle_node = heapNode.node;
|
|
path_upper_bound = path_weight;
|
|
}
|
|
}
|
|
|
|
// Relax outgoing edges from node
|
|
relaxOutgoingEdges<DIRECTION>(facade, forward_heap, heapNode, args...);
|
|
}
|
|
|
|
// With (s, middle, t) we trace back the paths middle -> s and middle -> t.
|
|
// This gives us a packed path (node ids) from the base graph around s and t,
|
|
// and overlay node ids otherwise. We then have to unpack the overlay clique
|
|
// edges by recursively descending unpacking the path down to the base graph.
|
|
|
|
using UnpackedNodes = std::vector<NodeID>;
|
|
using UnpackedEdges = std::vector<EdgeID>;
|
|
|
|
struct UnpackedPath
|
|
{
|
|
EdgeWeight weight;
|
|
UnpackedNodes nodes;
|
|
UnpackedEdges edges;
|
|
};
|
|
|
|
template <typename Algorithm, typename Heap, typename... Args>
|
|
std::optional<std::pair<NodeID, EdgeWeight>> runSearch(const DataFacade<Algorithm> &facade,
|
|
Heap &forward_heap,
|
|
Heap &reverse_heap,
|
|
const std::vector<NodeID> &force_step_nodes,
|
|
EdgeWeight weight_upper_bound,
|
|
const Args &...args)
|
|
{
|
|
if (forward_heap.Empty() || reverse_heap.Empty())
|
|
{
|
|
return {};
|
|
}
|
|
|
|
BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
|
|
BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);
|
|
|
|
// run two-Target Dijkstra routing step.
|
|
NodeID middle = SPECIAL_NODEID;
|
|
EdgeWeight weight = weight_upper_bound;
|
|
EdgeWeight forward_heap_min = forward_heap.MinKey();
|
|
EdgeWeight reverse_heap_min = reverse_heap.MinKey();
|
|
while (forward_heap.Size() + reverse_heap.Size() > 0 &&
|
|
forward_heap_min + reverse_heap_min < weight)
|
|
{
|
|
if (!forward_heap.Empty())
|
|
{
|
|
routingStep<FORWARD_DIRECTION>(
|
|
facade, forward_heap, reverse_heap, middle, weight, force_step_nodes, args...);
|
|
if (!forward_heap.Empty())
|
|
forward_heap_min = forward_heap.MinKey();
|
|
}
|
|
if (!reverse_heap.Empty())
|
|
{
|
|
routingStep<REVERSE_DIRECTION>(
|
|
facade, reverse_heap, forward_heap, middle, weight, force_step_nodes, args...);
|
|
if (!reverse_heap.Empty())
|
|
reverse_heap_min = reverse_heap.MinKey();
|
|
}
|
|
};
|
|
|
|
// No path found for both target nodes?
|
|
if (weight >= weight_upper_bound || SPECIAL_NODEID == middle)
|
|
{
|
|
return {};
|
|
}
|
|
|
|
return {{middle, weight}};
|
|
}
|
|
|
|
template <typename Algorithm, typename... Args>
|
|
UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
|
|
const DataFacade<Algorithm> &facade,
|
|
typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
|
|
typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
|
|
const std::vector<NodeID> &force_step_nodes,
|
|
EdgeWeight weight_upper_bound,
|
|
const Args &...args)
|
|
{
|
|
auto searchResult = runSearch(
|
|
facade, forward_heap, reverse_heap, force_step_nodes, weight_upper_bound, args...);
|
|
if (!searchResult)
|
|
{
|
|
return {INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>()};
|
|
}
|
|
|
|
auto [middle, weight] = *searchResult;
|
|
|
|
const auto &partition = facade.GetMultiLevelPartition();
|
|
|
|
// Get packed path as edges {from node ID, to node ID, from_clique_arc}
|
|
auto packed_path = retrievePackedPathFromHeap(forward_heap, reverse_heap, middle);
|
|
|
|
// Beware the edge case when start, middle, end are all the same.
|
|
// In this case we return a single node, no edges. We also don't unpack.
|
|
const NodeID source_node = !packed_path.empty() ? std::get<0>(packed_path.front()) : middle;
|
|
|
|
// Unpack path
|
|
std::vector<NodeID> unpacked_nodes;
|
|
std::vector<EdgeID> unpacked_edges;
|
|
unpacked_nodes.reserve(packed_path.size());
|
|
unpacked_edges.reserve(packed_path.size());
|
|
|
|
unpacked_nodes.push_back(source_node);
|
|
|
|
for (auto const &packed_edge : packed_path)
|
|
{
|
|
auto [source, target, overlay_edge] = packed_edge;
|
|
if (!overlay_edge)
|
|
{ // a base graph edge
|
|
unpacked_nodes.push_back(target);
|
|
unpacked_edges.push_back(facade.FindEdge(source, target));
|
|
}
|
|
else
|
|
{ // an overlay graph edge
|
|
LevelID level = getNodeQueryLevel(partition, source, args...);
|
|
CellID parent_cell_id = partition.GetCell(level, source);
|
|
BOOST_ASSERT(parent_cell_id == partition.GetCell(level, target));
|
|
|
|
LevelID sublevel = level - 1;
|
|
|
|
// Here heaps can be reused, let's go deeper!
|
|
forward_heap.Clear();
|
|
reverse_heap.Clear();
|
|
forward_heap.Insert(source, {0}, {source});
|
|
reverse_heap.Insert(target, {0}, {target});
|
|
|
|
auto unpacked_subpath = search(engine_working_data,
|
|
facade,
|
|
forward_heap,
|
|
reverse_heap,
|
|
force_step_nodes,
|
|
INVALID_EDGE_WEIGHT,
|
|
sublevel,
|
|
parent_cell_id);
|
|
BOOST_ASSERT(!unpacked_subpath.edges.empty());
|
|
BOOST_ASSERT(unpacked_subpath.nodes.size() > 1);
|
|
BOOST_ASSERT(unpacked_subpath.nodes.front() == source);
|
|
BOOST_ASSERT(unpacked_subpath.nodes.back() == target);
|
|
unpacked_nodes.insert(unpacked_nodes.end(),
|
|
std::next(unpacked_subpath.nodes.begin()),
|
|
unpacked_subpath.nodes.end());
|
|
unpacked_edges.insert(
|
|
unpacked_edges.end(), unpacked_subpath.edges.begin(), unpacked_subpath.edges.end());
|
|
}
|
|
}
|
|
|
|
return {weight, std::move(unpacked_nodes), std::move(unpacked_edges)};
|
|
}
|
|
|
|
template <typename Algorithm, typename... Args>
|
|
EdgeDistance
|
|
searchDistance(SearchEngineData<Algorithm> &,
|
|
const DataFacade<Algorithm> &facade,
|
|
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
|
|
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &reverse_heap,
|
|
const std::vector<NodeID> &force_step_nodes,
|
|
EdgeWeight weight_upper_bound,
|
|
const Args &...args)
|
|
{
|
|
|
|
auto searchResult = runSearch(
|
|
facade, forward_heap, reverse_heap, force_step_nodes, weight_upper_bound, args...);
|
|
if (!searchResult)
|
|
{
|
|
return INVALID_EDGE_DISTANCE;
|
|
}
|
|
|
|
auto [middle, _] = *searchResult;
|
|
|
|
auto distance = forward_heap.GetData(middle).distance + reverse_heap.GetData(middle).distance;
|
|
|
|
return distance;
|
|
}
|
|
|
|
// Alias to be compatible with the CH-based search
|
|
template <typename Algorithm, typename PhantomEndpointT>
|
|
inline void search(SearchEngineData<Algorithm> &engine_working_data,
|
|
const DataFacade<Algorithm> &facade,
|
|
typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
|
|
typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
|
|
EdgeWeight &weight,
|
|
std::vector<NodeID> &unpacked_nodes,
|
|
const std::vector<NodeID> &force_step_nodes,
|
|
const PhantomEndpointT &endpoints,
|
|
const EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
|
|
{
|
|
// TODO: change search calling interface to use unpacked_edges result
|
|
auto unpacked_path = search(engine_working_data,
|
|
facade,
|
|
forward_heap,
|
|
reverse_heap,
|
|
force_step_nodes,
|
|
weight_upper_bound,
|
|
endpoints);
|
|
weight = unpacked_path.weight;
|
|
unpacked_nodes = std::move(unpacked_path.nodes);
|
|
}
|
|
|
|
// TODO: refactor CH-related stub to use unpacked_edges
|
|
template <typename RandomIter, typename FacadeT>
|
|
void unpackPath(const FacadeT &facade,
|
|
RandomIter packed_path_begin,
|
|
RandomIter packed_path_end,
|
|
const PhantomEndpoints &route_endpoints,
|
|
std::vector<PathData> &unpacked_path)
|
|
{
|
|
const auto nodes_number = std::distance(packed_path_begin, packed_path_end);
|
|
BOOST_ASSERT(nodes_number > 0);
|
|
|
|
std::vector<NodeID> unpacked_nodes;
|
|
std::vector<EdgeID> unpacked_edges;
|
|
unpacked_nodes.reserve(nodes_number);
|
|
unpacked_edges.reserve(nodes_number);
|
|
|
|
unpacked_nodes.push_back(*packed_path_begin);
|
|
if (nodes_number > 1)
|
|
{
|
|
util::for_each_pair(
|
|
packed_path_begin,
|
|
packed_path_end,
|
|
[&facade, &unpacked_nodes, &unpacked_edges](const auto from, const auto to)
|
|
{
|
|
unpacked_nodes.push_back(to);
|
|
unpacked_edges.push_back(facade.FindEdge(from, to));
|
|
});
|
|
}
|
|
|
|
annotatePath(facade, route_endpoints, unpacked_nodes, unpacked_edges, unpacked_path);
|
|
}
|
|
|
|
template <typename Algorithm>
|
|
double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
|
|
const DataFacade<Algorithm> &facade,
|
|
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
|
|
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &reverse_heap,
|
|
const PhantomNode &source_phantom,
|
|
const PhantomNode &target_phantom,
|
|
EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
|
|
{
|
|
forward_heap.Clear();
|
|
reverse_heap.Clear();
|
|
|
|
if (source_phantom.IsValidForwardSource())
|
|
{
|
|
forward_heap.Insert(source_phantom.forward_segment_id.id,
|
|
EdgeWeight{0} - source_phantom.GetForwardWeightPlusOffset(),
|
|
{source_phantom.forward_segment_id.id,
|
|
false,
|
|
EdgeDistance{0} - source_phantom.GetForwardDistance()});
|
|
}
|
|
|
|
if (source_phantom.IsValidReverseSource())
|
|
{
|
|
forward_heap.Insert(source_phantom.reverse_segment_id.id,
|
|
EdgeWeight{0} - source_phantom.GetReverseWeightPlusOffset(),
|
|
{source_phantom.reverse_segment_id.id,
|
|
false,
|
|
EdgeDistance{0} - source_phantom.GetReverseDistance()});
|
|
}
|
|
|
|
if (target_phantom.IsValidForwardTarget())
|
|
{
|
|
reverse_heap.Insert(
|
|
target_phantom.forward_segment_id.id,
|
|
target_phantom.GetForwardWeightPlusOffset(),
|
|
{target_phantom.forward_segment_id.id, false, target_phantom.GetForwardDistance()});
|
|
}
|
|
|
|
if (target_phantom.IsValidReverseTarget())
|
|
{
|
|
reverse_heap.Insert(
|
|
target_phantom.reverse_segment_id.id,
|
|
target_phantom.GetReverseWeightPlusOffset(),
|
|
{target_phantom.reverse_segment_id.id, false, target_phantom.GetReverseDistance()});
|
|
}
|
|
|
|
const PhantomEndpoints endpoints{source_phantom, target_phantom};
|
|
|
|
auto distance = searchDistance(
|
|
engine_working_data, facade, forward_heap, reverse_heap, {}, weight_upper_bound, endpoints);
|
|
|
|
if (distance == INVALID_EDGE_DISTANCE)
|
|
{
|
|
return std::numeric_limits<double>::max();
|
|
}
|
|
return from_alias<double>(distance);
|
|
}
|
|
|
|
} // namespace osrm::engine::routing_algorithms::mld
|
|
|
|
#endif // OSRM_ENGINE_ROUTING_BASE_MLD_HPP
|