437 lines
20 KiB
C++
437 lines
20 KiB
C++
#ifndef ROUTING_BASE_HPP
|
|
#define ROUTING_BASE_HPP
|
|
|
|
#include "extractor/guidance/turn_instruction.hpp"
|
|
#include "engine/datafacade/datafacade_base.hpp"
|
|
#include "engine/edge_unpacker.hpp"
|
|
#include "engine/internal_route_result.hpp"
|
|
#include "engine/search_engine_data.hpp"
|
|
#include "util/coordinate_calculation.hpp"
|
|
#include "util/guidance/turn_bearing.hpp"
|
|
#include "util/typedefs.hpp"
|
|
|
|
#include <boost/assert.hpp>
|
|
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
|
|
#include <algorithm>
|
|
#include <functional>
|
|
#include <iterator>
|
|
#include <memory>
|
|
#include <numeric>
|
|
#include <stack>
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
namespace osrm
|
|
{
|
|
namespace engine
|
|
{
|
|
|
|
namespace routing_algorithms
|
|
{
|
|
|
|
class BasicRoutingInterface
|
|
{
|
|
protected:
|
|
using EdgeData = datafacade::BaseDataFacade::EdgeData;
|
|
|
|
public:
|
|
/*
|
|
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(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
SearchEngineData::QueryHeap &forward_heap,
|
|
SearchEngineData::QueryHeap &reverse_heap,
|
|
NodeID &middle_node_id,
|
|
std::int32_t &upper_bound,
|
|
std::int32_t min_edge_offset,
|
|
const bool forward_direction,
|
|
const bool stalling,
|
|
const bool force_loop_forward,
|
|
const bool force_loop_reverse) const;
|
|
|
|
template <bool UseDuration>
|
|
EdgeWeight GetLoopWeight(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
NodeID node) const
|
|
{
|
|
EdgeWeight loop_weight = UseDuration ? MAXIMAL_EDGE_DURATION : INVALID_EDGE_WEIGHT;
|
|
for (auto edge : facade->GetAdjacentEdgeRange(node))
|
|
{
|
|
const auto &data = facade->GetEdgeData(edge);
|
|
if (data.forward)
|
|
{
|
|
const NodeID to = facade->GetTarget(edge);
|
|
if (to == node)
|
|
{
|
|
const auto value = UseDuration ? data.duration : data.weight;
|
|
loop_weight = std::min(loop_weight, value);
|
|
}
|
|
}
|
|
}
|
|
return loop_weight;
|
|
}
|
|
|
|
template <typename RandomIter>
|
|
void UnpackPath(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
RandomIter packed_path_begin,
|
|
RandomIter packed_path_end,
|
|
const PhantomNodes &phantom_node_pair,
|
|
std::vector<PathData> &unpacked_path) const
|
|
{
|
|
BOOST_ASSERT(std::distance(packed_path_begin, packed_path_end) > 0);
|
|
|
|
const bool start_traversed_in_reverse =
|
|
(*packed_path_begin != phantom_node_pair.source_phantom.forward_segment_id.id);
|
|
const bool target_traversed_in_reverse =
|
|
(*std::prev(packed_path_end) != phantom_node_pair.target_phantom.forward_segment_id.id);
|
|
|
|
BOOST_ASSERT(*packed_path_begin == phantom_node_pair.source_phantom.forward_segment_id.id ||
|
|
*packed_path_begin == phantom_node_pair.source_phantom.reverse_segment_id.id);
|
|
BOOST_ASSERT(
|
|
*std::prev(packed_path_end) == phantom_node_pair.target_phantom.forward_segment_id.id ||
|
|
*std::prev(packed_path_end) == phantom_node_pair.target_phantom.reverse_segment_id.id);
|
|
|
|
UnpackCHPath(
|
|
*facade,
|
|
packed_path_begin,
|
|
packed_path_end,
|
|
[this,
|
|
&facade,
|
|
&unpacked_path,
|
|
&phantom_node_pair,
|
|
&start_traversed_in_reverse,
|
|
&target_traversed_in_reverse](std::pair<NodeID, NodeID> & /* edge */,
|
|
const EdgeData &edge_data) {
|
|
|
|
BOOST_ASSERT_MSG(!edge_data.shortcut, "original edge flagged as shortcut");
|
|
const auto name_index = facade->GetNameIndexFromEdgeID(edge_data.id);
|
|
const auto turn_instruction = facade->GetTurnInstructionForEdgeID(edge_data.id);
|
|
const extractor::TravelMode travel_mode =
|
|
(unpacked_path.empty() && start_traversed_in_reverse)
|
|
? phantom_node_pair.source_phantom.backward_travel_mode
|
|
: facade->GetTravelModeForEdgeID(edge_data.id);
|
|
|
|
const auto geometry_index = facade->GetGeometryIndexForEdgeID(edge_data.id);
|
|
std::vector<NodeID> id_vector;
|
|
|
|
std::vector<EdgeWeight> weight_vector;
|
|
std::vector<EdgeWeight> duration_vector;
|
|
std::vector<DatasourceID> datasource_vector;
|
|
if (geometry_index.forward)
|
|
{
|
|
id_vector = facade->GetUncompressedForwardGeometry(geometry_index.id);
|
|
weight_vector = facade->GetUncompressedForwardWeights(geometry_index.id);
|
|
duration_vector = facade->GetUncompressedForwardDurations(geometry_index.id);
|
|
datasource_vector =
|
|
facade->GetUncompressedForwardDatasources(geometry_index.id);
|
|
}
|
|
else
|
|
{
|
|
id_vector = facade->GetUncompressedReverseGeometry(geometry_index.id);
|
|
weight_vector = facade->GetUncompressedReverseWeights(geometry_index.id);
|
|
duration_vector = facade->GetUncompressedReverseDurations(geometry_index.id);
|
|
datasource_vector =
|
|
facade->GetUncompressedReverseDatasources(geometry_index.id);
|
|
}
|
|
BOOST_ASSERT(id_vector.size() > 0);
|
|
BOOST_ASSERT(datasource_vector.size() > 0);
|
|
BOOST_ASSERT(weight_vector.size() == id_vector.size() - 1);
|
|
BOOST_ASSERT(duration_vector.size() == id_vector.size() - 1);
|
|
const bool is_first_segment = unpacked_path.empty();
|
|
|
|
const std::size_t start_index =
|
|
(is_first_segment
|
|
? ((start_traversed_in_reverse)
|
|
? weight_vector.size() -
|
|
phantom_node_pair.source_phantom.fwd_segment_position - 1
|
|
: phantom_node_pair.source_phantom.fwd_segment_position)
|
|
: 0);
|
|
const std::size_t end_index = weight_vector.size();
|
|
|
|
BOOST_ASSERT(start_index >= 0);
|
|
BOOST_ASSERT(start_index < end_index);
|
|
for (std::size_t segment_idx = start_index; segment_idx < end_index; ++segment_idx)
|
|
{
|
|
unpacked_path.push_back(
|
|
PathData{id_vector[segment_idx + 1],
|
|
name_index,
|
|
weight_vector[segment_idx],
|
|
duration_vector[segment_idx],
|
|
extractor::guidance::TurnInstruction::NO_TURN(),
|
|
{{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
|
|
travel_mode,
|
|
INVALID_ENTRY_CLASSID,
|
|
datasource_vector[segment_idx],
|
|
util::guidance::TurnBearing(0),
|
|
util::guidance::TurnBearing(0)});
|
|
}
|
|
BOOST_ASSERT(unpacked_path.size() > 0);
|
|
if (facade->hasLaneData(edge_data.id))
|
|
unpacked_path.back().lane_data = facade->GetLaneData(edge_data.id);
|
|
|
|
unpacked_path.back().entry_classid = facade->GetEntryClassID(edge_data.id);
|
|
unpacked_path.back().turn_instruction = turn_instruction;
|
|
unpacked_path.back().duration_until_turn +=
|
|
facade->GetDurationPenaltyForEdgeID(edge_data.id);
|
|
unpacked_path.back().weight_until_turn +=
|
|
facade->GetWeightPenaltyForEdgeID(edge_data.id);
|
|
unpacked_path.back().pre_turn_bearing = facade->PreTurnBearing(edge_data.id);
|
|
unpacked_path.back().post_turn_bearing = facade->PostTurnBearing(edge_data.id);
|
|
});
|
|
|
|
std::size_t start_index = 0, end_index = 0;
|
|
std::vector<unsigned> id_vector;
|
|
std::vector<EdgeWeight> weight_vector;
|
|
std::vector<EdgeWeight> duration_vector;
|
|
std::vector<DatasourceID> datasource_vector;
|
|
const bool is_local_path = (phantom_node_pair.source_phantom.packed_geometry_id ==
|
|
phantom_node_pair.target_phantom.packed_geometry_id) &&
|
|
unpacked_path.empty();
|
|
|
|
if (target_traversed_in_reverse)
|
|
{
|
|
id_vector = facade->GetUncompressedReverseGeometry(
|
|
phantom_node_pair.target_phantom.packed_geometry_id);
|
|
|
|
weight_vector = facade->GetUncompressedReverseWeights(
|
|
phantom_node_pair.target_phantom.packed_geometry_id);
|
|
|
|
duration_vector = facade->GetUncompressedReverseDurations(
|
|
phantom_node_pair.target_phantom.packed_geometry_id);
|
|
|
|
datasource_vector = facade->GetUncompressedReverseDatasources(
|
|
phantom_node_pair.target_phantom.packed_geometry_id);
|
|
|
|
if (is_local_path)
|
|
{
|
|
start_index = weight_vector.size() -
|
|
phantom_node_pair.source_phantom.fwd_segment_position - 1;
|
|
}
|
|
end_index =
|
|
weight_vector.size() - phantom_node_pair.target_phantom.fwd_segment_position - 1;
|
|
}
|
|
else
|
|
{
|
|
if (is_local_path)
|
|
{
|
|
start_index = phantom_node_pair.source_phantom.fwd_segment_position;
|
|
}
|
|
end_index = phantom_node_pair.target_phantom.fwd_segment_position;
|
|
|
|
id_vector = facade->GetUncompressedForwardGeometry(
|
|
phantom_node_pair.target_phantom.packed_geometry_id);
|
|
|
|
weight_vector = facade->GetUncompressedForwardWeights(
|
|
phantom_node_pair.target_phantom.packed_geometry_id);
|
|
|
|
duration_vector = facade->GetUncompressedForwardDurations(
|
|
phantom_node_pair.target_phantom.packed_geometry_id);
|
|
|
|
datasource_vector = facade->GetUncompressedForwardDatasources(
|
|
phantom_node_pair.target_phantom.packed_geometry_id);
|
|
}
|
|
|
|
// Given the following compressed geometry:
|
|
// U---v---w---x---y---Z
|
|
// s t
|
|
// s: fwd_segment 0
|
|
// t: fwd_segment 3
|
|
// -> (U, v), (v, w), (w, x)
|
|
// note that (x, t) is _not_ included but needs to be added later.
|
|
for (std::size_t segment_idx = start_index; segment_idx != end_index;
|
|
(start_index < end_index ? ++segment_idx : --segment_idx))
|
|
{
|
|
BOOST_ASSERT(segment_idx < id_vector.size() - 1);
|
|
BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0);
|
|
unpacked_path.push_back(PathData{
|
|
id_vector[start_index < end_index ? segment_idx + 1 : segment_idx - 1],
|
|
phantom_node_pair.target_phantom.name_id,
|
|
weight_vector[segment_idx],
|
|
duration_vector[segment_idx],
|
|
extractor::guidance::TurnInstruction::NO_TURN(),
|
|
{{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
|
|
target_traversed_in_reverse ? phantom_node_pair.target_phantom.backward_travel_mode
|
|
: phantom_node_pair.target_phantom.forward_travel_mode,
|
|
INVALID_ENTRY_CLASSID,
|
|
datasource_vector[segment_idx],
|
|
util::guidance::TurnBearing(0),
|
|
util::guidance::TurnBearing(0)});
|
|
}
|
|
|
|
if (unpacked_path.size() > 0)
|
|
{
|
|
const auto source_weight = start_traversed_in_reverse
|
|
? phantom_node_pair.source_phantom.reverse_weight
|
|
: phantom_node_pair.source_phantom.forward_weight;
|
|
const auto source_duration = start_traversed_in_reverse
|
|
? phantom_node_pair.source_phantom.reverse_duration
|
|
: phantom_node_pair.source_phantom.forward_duration;
|
|
// The above code will create segments for (v, w), (w,x), (x, y) and (y, Z).
|
|
// However the first segment duration needs to be adjusted to the fact that the source
|
|
// phantom is in the middle of the segment. We do this by subtracting v--s from the
|
|
// duration.
|
|
|
|
// Since it's possible duration_until_turn can be less than source_weight here if
|
|
// a negative enough turn penalty is used to modify this edge weight during
|
|
// osrm-contract, we clamp to 0 here so as not to return a negative duration
|
|
// for this segment.
|
|
|
|
// TODO this creates a scenario where it's possible the duration from a phantom
|
|
// node to the first turn would be the same as from end to end of a segment,
|
|
// which is obviously incorrect and not ideal...
|
|
unpacked_path.front().weight_until_turn =
|
|
std::max(unpacked_path.front().weight_until_turn - source_weight, 0);
|
|
unpacked_path.front().duration_until_turn =
|
|
std::max(unpacked_path.front().duration_until_turn - source_duration, 0);
|
|
}
|
|
|
|
// there is no equivalent to a node-based node in an edge-expanded graph.
|
|
// two equivalent routes may start (or end) at different node-based edges
|
|
// as they are added with the offset how much "weight" on the edge
|
|
// has already been traversed. Depending on offset one needs to remove
|
|
// the last node.
|
|
if (unpacked_path.size() > 1)
|
|
{
|
|
const std::size_t last_index = unpacked_path.size() - 1;
|
|
const std::size_t second_to_last_index = last_index - 1;
|
|
|
|
if (unpacked_path[last_index].turn_via_node ==
|
|
unpacked_path[second_to_last_index].turn_via_node)
|
|
{
|
|
unpacked_path.pop_back();
|
|
}
|
|
BOOST_ASSERT(!unpacked_path.empty());
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Unpacks a single edge (NodeID->NodeID) from the CH graph down to it's original non-shortcut
|
|
* route.
|
|
* @param from the node the CH edge starts at
|
|
* @param to the node the CH edge finishes at
|
|
* @param unpacked_path the sequence of original NodeIDs that make up the expanded CH edge
|
|
*/
|
|
void UnpackEdge(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
const NodeID from,
|
|
const NodeID to,
|
|
std::vector<NodeID> &unpacked_path) const;
|
|
|
|
void RetrievePackedPathFromHeap(const SearchEngineData::QueryHeap &forward_heap,
|
|
const SearchEngineData::QueryHeap &reverse_heap,
|
|
const NodeID middle_node_id,
|
|
std::vector<NodeID> &packed_path) const;
|
|
|
|
void RetrievePackedPathFromSingleHeap(const SearchEngineData::QueryHeap &search_heap,
|
|
const NodeID middle_node_id,
|
|
std::vector<NodeID> &packed_path) const;
|
|
|
|
// assumes that heaps are already setup correctly.
|
|
// ATTENTION: This only works if no additional offset is supplied next to the Phantom Node
|
|
// Offsets.
|
|
// In case additional offsets are supplied, you might have to force a loop first.
|
|
// A forced loop might be necessary, if source and target are on the same segment.
|
|
// If this is the case and the offsets of the respective direction are larger for the source
|
|
// than the target
|
|
// then a force loop is required (e.g. source_phantom.forward_segment_id ==
|
|
// target_phantom.forward_segment_id
|
|
// && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset())
|
|
// requires
|
|
// a force loop, if the heaps have been initialized with positive offsets.
|
|
void Search(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
SearchEngineData::QueryHeap &forward_heap,
|
|
SearchEngineData::QueryHeap &reverse_heap,
|
|
std::int32_t &weight,
|
|
std::vector<NodeID> &packed_leg,
|
|
const bool force_loop_forward,
|
|
const bool force_loop_reverse,
|
|
const int duration_upper_bound = INVALID_EDGE_WEIGHT) const;
|
|
|
|
// assumes that heaps are already setup correctly.
|
|
// A forced loop might be necessary, if source and target are on the same segment.
|
|
// If this is the case and the offsets of the respective direction are larger for the source
|
|
// than the target
|
|
// then a force loop is required (e.g. source_phantom.forward_segment_id ==
|
|
// target_phantom.forward_segment_id
|
|
// && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset())
|
|
// requires
|
|
// a force loop, if the heaps have been initialized with positive offsets.
|
|
void SearchWithCore(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
SearchEngineData::QueryHeap &forward_heap,
|
|
SearchEngineData::QueryHeap &reverse_heap,
|
|
SearchEngineData::QueryHeap &forward_core_heap,
|
|
SearchEngineData::QueryHeap &reverse_core_heap,
|
|
int &weight,
|
|
std::vector<NodeID> &packed_leg,
|
|
const bool force_loop_forward,
|
|
const bool force_loop_reverse,
|
|
int duration_upper_bound = INVALID_EDGE_WEIGHT) const;
|
|
|
|
bool NeedsLoopForward(const PhantomNode &source_phantom,
|
|
const PhantomNode &target_phantom) const;
|
|
|
|
bool NeedsLoopBackwards(const PhantomNode &source_phantom,
|
|
const PhantomNode &target_phantom) const;
|
|
|
|
double GetPathDistance(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
const std::vector<NodeID> &packed_path,
|
|
const PhantomNode &source_phantom,
|
|
const PhantomNode &target_phantom) const;
|
|
|
|
// Requires the heaps for be empty
|
|
// If heaps should be adjusted to be initialized outside of this function,
|
|
// the addition of force_loop parameters might be required
|
|
double
|
|
GetNetworkDistanceWithCore(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
SearchEngineData::QueryHeap &forward_heap,
|
|
SearchEngineData::QueryHeap &reverse_heap,
|
|
SearchEngineData::QueryHeap &forward_core_heap,
|
|
SearchEngineData::QueryHeap &reverse_core_heap,
|
|
const PhantomNode &source_phantom,
|
|
const PhantomNode &target_phantom,
|
|
int duration_upper_bound = INVALID_EDGE_WEIGHT) const;
|
|
|
|
// Requires the heaps for be empty
|
|
// If heaps should be adjusted to be initialized outside of this function,
|
|
// the addition of force_loop parameters might be required
|
|
double GetNetworkDistance(const std::shared_ptr<const datafacade::BaseDataFacade> facade,
|
|
SearchEngineData::QueryHeap &forward_heap,
|
|
SearchEngineData::QueryHeap &reverse_heap,
|
|
const PhantomNode &source_phantom,
|
|
const PhantomNode &target_phantom,
|
|
int duration_upper_bound = INVALID_EDGE_WEIGHT) const;
|
|
};
|
|
|
|
} // namespace routing_algorithms
|
|
} // namespace engine
|
|
} // namespace osrm
|
|
|
|
#endif // ROUTING_BASE_HPP
|