Permalink
Switch branches/tags
5-20-0-alpha-3 5.0 5.1 5.2 5.3 5.4 5.5 5.6 5.7 5.8 5.9 5.10 5.11 5.12 5.13 5.14 5.15 5.16 5.17 5.18 5.19 5.20 45-degree-granularity KnockSoftware-network-distance-comparison algorihm/astar api/goodbye_json api/json_match api/route_json_try api/route_json bad-refactor bearing-docs bicycle/new_tests ch_isochrone_poc changelog_5.15.2 collapse-forward-first-destination compute-annotations-for-table-at-runtime conditionals/disable danpat_cache_distances danpat_enable_all_unit_tests danpat_fix_windows_1 danpat_optional_simple_snapping danpat_optional_waypoints danpat_pin_yarn danpat_snapping_for_matching danpat_table_noroute_estimate danpat_5.18_shm_fixes debug/runtime_parameters demote-trunk distances docker-ci-build-type experimental/connectivity experimental/route-sketch experimental/route_relations experimental/vtzero feature/docker-on-ubuntu feature/general_manytomany feature/grasp feature/lua_impedance feature/pearce_scc feature/stops_in_turns fewer_json_copies fix-docs-links fix/bearings-regression fix/boost_po_165 fix/lua-dirs fix_maxspeed fix/no-debug-log fix/obvious-2 fix/pkg-config-version fix/subcrash fix/undedupe fix/uturn_bearing_after fix/3076 fix/3311 fix/4286 fix/4984 gdg_trace ghoshkaj_mmaperize guidance/geojson_logging_forks guidance/geojson_logging_obvious guidance/silence-name-changes guidance/silver-bullet guidance/sub-exits guidance/use-turn-tag hack/less-stxxl implement-cache intersection-validation-pass invalidate-conditional-turns isochrone_tool junction-circular kdiluca-internal-intersection kdiluca-turn-angle-adjustments libosrm-forward-base-params libosrm-namespace-pollution macau_hongkong maneuver_straight master mld-distances-tbd moar_flames motorway/continue multi_exit_roundabout_nodes new-branch-for-nld-distances nodejs-unlimited nosudo optimize/less-mutex osrm-routed.js osrm-runner-dev-deps osx-circleci pearce_scc penalize-uturns-viterbi permissive-routability potayto_potahto pr_checklist_improvement pr_template_changelog pre5-api process_relevant_nodes_only profile/cross_traffic_penalty profile_testing profile/tuning_baseline profiles_routes_handler prototype/dump_lua prototype/fast-geojson prototype/normalized_graph real_osm_cucumber refactor/bicycle_better_push_handling refactor/bike_profile_cleanup refactor/ebgf-guidance refactor_edge_based_nodes refactor/sharedmem-locking refactor/simplify_ebn refactor/start-points renumber-remove-dummy-edges reset_master restrictions rewrite/profiles road-priorities rtree_str_packing service_road_access shared-storage-cache snapping_name_hint stabilize_alternative_tests stacktraces table_plugin_benchmark temporary-demo-server-state test-updates test-walk test/assertion_in_compare test/no-flush test_valhalla_guidance tidy-traces units-for-profiles uturn-restriction v5.20.1 v8 variant-1-1-4 version-5-6-0-cmakelists version_from_packagejson way_locations waypoint_bug
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
438 lines (378 sloc) 18.1 KB
#ifndef OSRM_ENGINE_ROUTING_BASE_HPP
#define OSRM_ENGINE_ROUTING_BASE_HPP
#include "guidance/turn_bearing.hpp"
#include "guidance/turn_instruction.hpp"
#include "engine/algorithm.hpp"
#include "engine/datafacade.hpp"
#include "engine/internal_route_result.hpp"
#include "engine/phantom_node.hpp"
#include "engine/search_engine_data.hpp"
#include "util/coordinate_calculation.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
{
static constexpr bool FORWARD_DIRECTION = true;
static constexpr bool REVERSE_DIRECTION = false;
static constexpr bool DO_NOT_FORCE_LOOPS = false;
bool needsLoopForward(const PhantomNode &source_phantom, const PhantomNode &target_phantom);
bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &target_phantom);
bool needsLoopForward(const PhantomNodes &phantoms);
bool needsLoopBackwards(const PhantomNodes &phantoms);
template <typename Heap>
void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
{
const auto &source = nodes.source_phantom;
if (source.IsValidForwardSource())
{
forward_heap.Insert(source.forward_segment_id.id,
-source.GetForwardWeightPlusOffset(),
source.forward_segment_id.id);
}
if (source.IsValidReverseSource())
{
forward_heap.Insert(source.reverse_segment_id.id,
-source.GetReverseWeightPlusOffset(),
source.reverse_segment_id.id);
}
const auto &target = nodes.target_phantom;
if (target.IsValidForwardTarget())
{
reverse_heap.Insert(target.forward_segment_id.id,
target.GetForwardWeightPlusOffset(),
target.forward_segment_id.id);
}
if (target.IsValidReverseTarget())
{
reverse_heap.Insert(target.reverse_segment_id.id,
target.GetReverseWeightPlusOffset(),
target.reverse_segment_id.id);
}
}
template <typename ManyToManyQueryHeap>
void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
{
if (phantom_node.IsValidForwardSource())
{
heap.Insert(phantom_node.forward_segment_id.id,
-phantom_node.GetForwardWeightPlusOffset(),
{phantom_node.forward_segment_id.id,
-phantom_node.GetForwardDuration(),
-phantom_node.GetForwardDistance()});
}
if (phantom_node.IsValidReverseSource())
{
heap.Insert(phantom_node.reverse_segment_id.id,
-phantom_node.GetReverseWeightPlusOffset(),
{phantom_node.reverse_segment_id.id,
-phantom_node.GetReverseDuration(),
-phantom_node.GetReverseDistance()});
}
}
template <typename ManyToManyQueryHeap>
void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
{
if (phantom_node.IsValidForwardTarget())
{
heap.Insert(phantom_node.forward_segment_id.id,
phantom_node.GetForwardWeightPlusOffset(),
{phantom_node.forward_segment_id.id,
phantom_node.GetForwardDuration(),
phantom_node.GetForwardDistance()});
}
if (phantom_node.IsValidReverseTarget())
{
heap.Insert(phantom_node.reverse_segment_id.id,
phantom_node.GetReverseWeightPlusOffset(),
{phantom_node.reverse_segment_id.id,
phantom_node.GetReverseDuration(),
phantom_node.GetReverseDistance()});
}
}
template <typename FacadeT>
void annotatePath(const FacadeT &facade,
const PhantomNodes &phantom_node_pair,
const std::vector<NodeID> &unpacked_nodes,
const std::vector<EdgeID> &unpacked_edges,
std::vector<PathData> &unpacked_path)
{
BOOST_ASSERT(!unpacked_nodes.empty());
BOOST_ASSERT(unpacked_nodes.size() == unpacked_edges.size() + 1);
const auto source_node_id = unpacked_nodes.front();
const auto target_node_id = unpacked_nodes.back();
const bool start_traversed_in_reverse =
phantom_node_pair.source_phantom.forward_segment_id.id != source_node_id;
const bool target_traversed_in_reverse =
phantom_node_pair.target_phantom.forward_segment_id.id != target_node_id;
BOOST_ASSERT(phantom_node_pair.source_phantom.forward_segment_id.id == source_node_id ||
phantom_node_pair.source_phantom.reverse_segment_id.id == source_node_id);
BOOST_ASSERT(phantom_node_pair.target_phantom.forward_segment_id.id == target_node_id ||
phantom_node_pair.target_phantom.reverse_segment_id.id == target_node_id);
// datastructures to hold extracted data from geometry
std::vector<NodeID> id_vector;
std::vector<SegmentWeight> weight_vector;
std::vector<SegmentDuration> duration_vector;
std::vector<DatasourceID> datasource_vector;
const auto get_segment_geometry = [&](const auto geometry_index) {
const auto copy = [](auto &vector, const auto range) {
vector.resize(range.size());
std::copy(range.begin(), range.end(), vector.begin());
};
if (geometry_index.forward)
{
copy(id_vector, facade.GetUncompressedForwardGeometry(geometry_index.id));
copy(weight_vector, facade.GetUncompressedForwardWeights(geometry_index.id));
copy(duration_vector, facade.GetUncompressedForwardDurations(geometry_index.id));
copy(datasource_vector, facade.GetUncompressedForwardDatasources(geometry_index.id));
}
else
{
copy(id_vector, facade.GetUncompressedReverseGeometry(geometry_index.id));
copy(weight_vector, facade.GetUncompressedReverseWeights(geometry_index.id));
copy(duration_vector, facade.GetUncompressedReverseDurations(geometry_index.id));
copy(datasource_vector, facade.GetUncompressedReverseDatasources(geometry_index.id));
}
};
auto node_from = unpacked_nodes.begin(), node_last = std::prev(unpacked_nodes.end());
for (auto edge = unpacked_edges.begin(); node_from != node_last; ++node_from, ++edge)
{
const auto &edge_data = facade.GetEdgeData(*edge);
const auto turn_id = edge_data.turn_id; // edge-based graph edge index
const auto node_id = *node_from; // edge-based graph node index
const auto name_index = facade.GetNameIndex(node_id);
const bool is_segregated = facade.IsSegregated(node_id);
const auto turn_instruction = facade.GetTurnInstructionForEdgeID(turn_id);
const extractor::TravelMode travel_mode = facade.GetTravelMode(node_id);
const auto classes = facade.GetClassData(node_id);
const auto geometry_index = facade.GetGeometryIndex(node_id);
get_segment_geometry(geometry_index);
BOOST_ASSERT(id_vector.size() > 0);
BOOST_ASSERT(datasource_vector.size() > 0);
BOOST_ASSERT(weight_vector.size() + 1 == id_vector.size());
BOOST_ASSERT(duration_vector.size() + 1 == id_vector.size());
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();
bool is_left_hand_driving = facade.IsLeftHandDriving(node_id);
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{*node_from,
id_vector[segment_idx + 1],
name_index,
is_segregated,
static_cast<EdgeWeight>(weight_vector[segment_idx]),
0,
static_cast<EdgeDuration>(duration_vector[segment_idx]),
0,
guidance::TurnInstruction::NO_TURN(),
{{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
travel_mode,
classes,
EMPTY_ENTRY_CLASS,
datasource_vector[segment_idx],
osrm::guidance::TurnBearing(0),
osrm::guidance::TurnBearing(0),
is_left_hand_driving});
}
BOOST_ASSERT(unpacked_path.size() > 0);
if (facade.HasLaneData(turn_id))
unpacked_path.back().lane_data = facade.GetLaneData(turn_id);
const auto turn_duration = facade.GetDurationPenaltyForEdgeID(turn_id);
const auto turn_weight = facade.GetWeightPenaltyForEdgeID(turn_id);
unpacked_path.back().entry_class = facade.GetEntryClass(turn_id);
unpacked_path.back().turn_instruction = turn_instruction;
unpacked_path.back().duration_until_turn += turn_duration;
unpacked_path.back().duration_of_turn = turn_duration;
unpacked_path.back().weight_until_turn += turn_weight;
unpacked_path.back().weight_of_turn = turn_weight;
unpacked_path.back().pre_turn_bearing = facade.PreTurnBearing(turn_id);
unpacked_path.back().post_turn_bearing = facade.PostTurnBearing(turn_id);
}
std::size_t start_index = 0, end_index = 0;
const auto source_geometry_id = facade.GetGeometryIndex(source_node_id).id;
const auto target_geometry = facade.GetGeometryIndex(target_node_id);
const auto is_local_path = source_geometry_id == target_geometry.id && unpacked_path.empty();
get_segment_geometry(target_geometry);
if (target_traversed_in_reverse)
{
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;
}
// 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.
bool is_target_left_hand_driving = facade.IsLeftHandDriving(target_node_id);
for (std::size_t segment_idx = start_index; segment_idx != end_index;
(start_index < end_index ? ++segment_idx : --segment_idx))
{
BOOST_ASSERT(segment_idx < static_cast<std::size_t>(id_vector.size() - 1));
BOOST_ASSERT(facade.GetTravelMode(target_node_id) > 0);
unpacked_path.push_back(
PathData{target_node_id,
id_vector[start_index < end_index ? segment_idx + 1 : segment_idx - 1],
facade.GetNameIndex(target_node_id),
facade.IsSegregated(target_node_id),
static_cast<EdgeWeight>(weight_vector[segment_idx]),
0,
static_cast<EdgeDuration>(duration_vector[segment_idx]),
0,
guidance::TurnInstruction::NO_TURN(),
{{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
facade.GetTravelMode(target_node_id),
facade.GetClassData(target_node_id),
EMPTY_ENTRY_CLASS,
datasource_vector[segment_idx],
guidance::TurnBearing(0),
guidance::TurnBearing(0),
is_target_left_hand_driving});
}
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);
}
}
template <typename Algorithm>
double getPathDistance(const DataFacade<Algorithm> &facade,
const std::vector<PathData> unpacked_path,
const PhantomNode &source_phantom,
const PhantomNode &target_phantom)
{
using util::coordinate_calculation::detail::DEGREE_TO_RAD;
using util::coordinate_calculation::detail::EARTH_RADIUS;
double distance = 0;
double prev_lat =
static_cast<double>(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
double prev_lon =
static_cast<double>(util::toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD;
double prev_cos = std::cos(prev_lat);
for (const auto &p : unpacked_path)
{
const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node);
const double current_lat =
static_cast<double>(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
const double current_lon =
static_cast<double>(util::toFloating(current_coordinate.lon)) * DEGREE_TO_RAD;
const double current_cos = std::cos(current_lat);
const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
distance += EARTH_RADIUS * charv;
prev_lat = current_lat;
prev_lon = current_lon;
prev_cos = current_cos;
}
const double current_lat =
static_cast<double>(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
const double current_lon =
static_cast<double>(util::toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD;
const double current_cos = std::cos(current_lat);
const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
distance += EARTH_RADIUS * charv;
return distance;
}
template <typename AlgorithmT>
InternalRouteResult extractRoute(const DataFacade<AlgorithmT> &facade,
const EdgeWeight weight,
const PhantomNodes &phantom_nodes,
const std::vector<NodeID> &unpacked_nodes,
const std::vector<EdgeID> &unpacked_edges)
{
InternalRouteResult raw_route_data;
raw_route_data.segment_end_coordinates = {phantom_nodes};
// No path found for both target nodes?
if (INVALID_EDGE_WEIGHT == weight)
{
return raw_route_data;
}
raw_route_data.shortest_path_weight = weight;
raw_route_data.unpacked_path_segments.resize(1);
raw_route_data.source_traversed_in_reverse.push_back(
(unpacked_nodes.front() != phantom_nodes.source_phantom.forward_segment_id.id));
raw_route_data.target_traversed_in_reverse.push_back(
(unpacked_nodes.back() != phantom_nodes.target_phantom.forward_segment_id.id));
annotatePath(facade,
phantom_nodes,
unpacked_nodes,
unpacked_edges,
raw_route_data.unpacked_path_segments.front());
return raw_route_data;
}
template <typename FacadeT> EdgeDistance computeEdgeDistance(const FacadeT &facade, NodeID node_id)
{
const auto geometry_index = facade.GetGeometryIndex(node_id);
EdgeDistance total_distance = 0.0;
auto geometry_range = facade.GetUncompressedForwardGeometry(geometry_index.id);
for (auto current = geometry_range.begin(); current < geometry_range.end() - 1; ++current)
{
total_distance += util::coordinate_calculation::fccApproximateDistance(
facade.GetCoordinateOfNode(*current), facade.GetCoordinateOfNode(*std::next(current)));
}
return total_distance;
}
} // namespace routing_algorithms
} // namespace engine
} // namespace osrm
#endif // OSRM_ENGINE_ROUTING_BASE_HPP