406 lines
18 KiB
C++
406 lines
18 KiB
C++
#include "extractor/guidance/intersection_generator.hpp"
|
|
|
|
#include "extractor/geojson_debug_policies.hpp"
|
|
#include "util/geojson_debug_logger.hpp"
|
|
|
|
#include "util/bearing.hpp"
|
|
#include "util/coordinate_calculation.hpp"
|
|
#include "util/log.hpp"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <functional> // mem_fn
|
|
#include <limits>
|
|
#include <numeric>
|
|
#include <utility>
|
|
|
|
#include <boost/range/algorithm/count_if.hpp>
|
|
|
|
namespace osrm
|
|
{
|
|
namespace extractor
|
|
{
|
|
namespace guidance
|
|
{
|
|
namespace
|
|
{
|
|
const constexpr bool USE_LOW_PRECISION_MODE = true;
|
|
// the inverse of use low precision mode
|
|
const constexpr bool USE_HIGH_PRECISION_MODE = !USE_LOW_PRECISION_MODE;
|
|
}
|
|
|
|
IntersectionGenerator::IntersectionGenerator(
|
|
const util::NodeBasedDynamicGraph &node_based_graph,
|
|
const RestrictionMap &restriction_map,
|
|
const std::unordered_set<NodeID> &barrier_nodes,
|
|
const std::vector<QueryNode> &node_info_list,
|
|
const CompressedEdgeContainer &compressed_edge_container)
|
|
: node_based_graph(node_based_graph), restriction_map(restriction_map),
|
|
barrier_nodes(barrier_nodes), node_info_list(node_info_list),
|
|
coordinate_extractor(node_based_graph, compressed_edge_container, node_info_list)
|
|
{
|
|
}
|
|
|
|
IntersectionView IntersectionGenerator::operator()(const NodeID from_node,
|
|
const EdgeID via_eid) const
|
|
{
|
|
return GetConnectedRoads(from_node, via_eid, USE_HIGH_PRECISION_MODE);
|
|
}
|
|
|
|
IntersectionShape
|
|
IntersectionGenerator::ComputeIntersectionShape(const NodeID node_at_center_of_intersection,
|
|
const boost::optional<NodeID> sorting_base,
|
|
const bool use_low_precision_angles) const
|
|
{
|
|
IntersectionShape intersection;
|
|
// reserve enough items (+ the possibly missing u-turn edge)
|
|
const auto intersection_degree = node_based_graph.GetOutDegree(node_at_center_of_intersection);
|
|
intersection.reserve(intersection_degree);
|
|
const util::Coordinate turn_coordinate = node_info_list[node_at_center_of_intersection];
|
|
|
|
// number of lanes at the intersection changes how far we look down the road
|
|
const auto edge_range = node_based_graph.GetAdjacentEdgeRange(node_at_center_of_intersection);
|
|
const auto max_lanes_intersection = std::accumulate(
|
|
edge_range.begin(),
|
|
edge_range.end(),
|
|
std::uint8_t{0},
|
|
[this](const auto current_max, const auto current_eid) {
|
|
return std::max(
|
|
current_max,
|
|
node_based_graph.GetEdgeData(current_eid).road_classification.GetNumberOfLanes());
|
|
});
|
|
|
|
for (const EdgeID edge_connected_to_intersection :
|
|
node_based_graph.GetAdjacentEdgeRange(node_at_center_of_intersection))
|
|
{
|
|
BOOST_ASSERT(edge_connected_to_intersection != SPECIAL_EDGEID);
|
|
const NodeID to_node = node_based_graph.GetTarget(edge_connected_to_intersection);
|
|
double bearing = 0.;
|
|
|
|
auto coordinates = coordinate_extractor.GetCoordinatesAlongRoad(
|
|
node_at_center_of_intersection, edge_connected_to_intersection, !INVERT, to_node);
|
|
|
|
const auto segment_length = util::coordinate_calculation::getLength(
|
|
coordinates.begin(),
|
|
coordinates.end(),
|
|
util::coordinate_calculation::haversineDistance);
|
|
|
|
const auto extract_coordinate = [&](const NodeID from_node,
|
|
const EdgeID via_eid,
|
|
const bool traversed_in_reverse,
|
|
const NodeID to_node) {
|
|
return (use_low_precision_angles || intersection_degree <= 2)
|
|
? coordinate_extractor.GetCoordinateCloseToTurn(
|
|
from_node, via_eid, traversed_in_reverse, to_node)
|
|
: coordinate_extractor.ExtractRepresentativeCoordinate(
|
|
from_node,
|
|
via_eid,
|
|
traversed_in_reverse,
|
|
to_node,
|
|
max_lanes_intersection,
|
|
std::move(coordinates));
|
|
};
|
|
|
|
// we have to look down the road a bit to get the correct turn
|
|
const auto coordinate_along_edge_leaving = extract_coordinate(
|
|
node_at_center_of_intersection, edge_connected_to_intersection, !INVERT, to_node);
|
|
|
|
bearing =
|
|
util::coordinate_calculation::bearing(turn_coordinate, coordinate_along_edge_leaving);
|
|
|
|
// OSM data sometimes contains duplicated nodes with identical coordinates, or
|
|
// because of coordinate precision rounding, end up at the same coordinate.
|
|
// It's impossible to calculate a bearing between these, so we log a warning
|
|
// that the data should be checked.
|
|
// The bearing calculation should return 0 in these cases, which may not be correct,
|
|
// but is at least not random.
|
|
if (turn_coordinate == coordinate_along_edge_leaving)
|
|
{
|
|
util::Log(logDEBUG) << "Zero length segment at " << coordinate_along_edge_leaving
|
|
<< " could cause invalid intersection exit bearing.";
|
|
BOOST_ASSERT(std::abs(bearing) <= 0.1);
|
|
}
|
|
|
|
intersection.push_back({edge_connected_to_intersection, bearing, segment_length});
|
|
}
|
|
|
|
if (!intersection.empty())
|
|
{
|
|
const auto base_bearing = [&]() {
|
|
if (sorting_base)
|
|
{
|
|
const auto itr =
|
|
std::find_if(intersection.begin(),
|
|
intersection.end(),
|
|
[&](const IntersectionShapeData &data) {
|
|
return node_based_graph.GetTarget(data.eid) == *sorting_base;
|
|
});
|
|
if (itr != intersection.end())
|
|
return util::bearing::reverse(itr->bearing);
|
|
}
|
|
return util::bearing::reverse(intersection.begin()->bearing);
|
|
}();
|
|
std::sort(intersection.begin(),
|
|
intersection.end(),
|
|
makeCompareShapeDataAngleToBearing(base_bearing));
|
|
}
|
|
return intersection;
|
|
}
|
|
|
|
// a
|
|
// |
|
|
// |
|
|
// v
|
|
// For an intersection from_node --via_eid--> turn_node ----> c
|
|
// ^
|
|
// |
|
|
// |
|
|
// b
|
|
// This functions returns _all_ turns as if the graph was undirected.
|
|
// That means we not only get (from_node, turn_node, c) in the above example
|
|
// but also (from_node, turn_node, a), (from_node, turn_node, b). These turns are
|
|
// marked as invalid and only needed for intersection classification.
|
|
IntersectionView IntersectionGenerator::GetConnectedRoads(const NodeID from_node,
|
|
const EdgeID via_eid,
|
|
const bool use_low_precision_angles) const
|
|
{
|
|
// make sure the via-eid is valid
|
|
BOOST_ASSERT([this](const NodeID from_node, const EdgeID via_eid) {
|
|
const auto range = node_based_graph.GetAdjacentEdgeRange(from_node);
|
|
return range.front() <= via_eid && via_eid <= range.back();
|
|
}(from_node, via_eid));
|
|
|
|
auto intersection = ComputeIntersectionShape(
|
|
node_based_graph.GetTarget(via_eid), boost::none, use_low_precision_angles);
|
|
return TransformIntersectionShapeIntoView(from_node, via_eid, std::move(intersection));
|
|
}
|
|
|
|
IntersectionGenerationParameters
|
|
IntersectionGenerator::SkipDegreeTwoNodes(const NodeID starting_node, const EdgeID via_edge) const
|
|
{
|
|
NodeID query_node = starting_node;
|
|
EdgeID query_edge = via_edge;
|
|
|
|
const auto get_next_edge = [this](const NodeID from, const EdgeID via) {
|
|
const NodeID new_node = node_based_graph.GetTarget(via);
|
|
BOOST_ASSERT(node_based_graph.GetOutDegree(new_node) == 2);
|
|
const EdgeID begin_edges_new_node = node_based_graph.BeginEdges(new_node);
|
|
return (node_based_graph.GetTarget(begin_edges_new_node) == from) ? begin_edges_new_node + 1
|
|
: begin_edges_new_node;
|
|
};
|
|
|
|
std::unordered_set<NodeID> visited_nodes;
|
|
// skip trivial nodes without generating the intersection in between, stop at the very first
|
|
// intersection of degree > 2
|
|
while (0 == visited_nodes.count(query_node) &&
|
|
2 == node_based_graph.GetOutDegree(node_based_graph.GetTarget(query_edge)))
|
|
{
|
|
visited_nodes.insert(query_node);
|
|
const auto next_node = node_based_graph.GetTarget(query_edge);
|
|
const auto next_edge = get_next_edge(query_node, query_edge);
|
|
|
|
query_node = next_node;
|
|
query_edge = next_edge;
|
|
|
|
if (!node_based_graph.GetEdgeData(query_edge)
|
|
.IsCompatibleTo(node_based_graph.GetEdgeData(next_edge)) ||
|
|
node_based_graph.GetTarget(next_edge) == starting_node)
|
|
break;
|
|
}
|
|
|
|
return {query_node, query_edge};
|
|
}
|
|
|
|
IntersectionView IntersectionGenerator::TransformIntersectionShapeIntoView(
|
|
const NodeID previous_node,
|
|
const EdgeID entering_via_edge,
|
|
const IntersectionShape &intersection_shape) const
|
|
{
|
|
// requires a copy of the intersection
|
|
return TransformIntersectionShapeIntoView(previous_node,
|
|
entering_via_edge,
|
|
intersection_shape, // creates a copy
|
|
intersection_shape, // reference to local
|
|
{}); // empty vector of performed merges
|
|
}
|
|
|
|
IntersectionView IntersectionGenerator::TransformIntersectionShapeIntoView(
|
|
const NodeID previous_node,
|
|
const EdgeID entering_via_edge,
|
|
const IntersectionShape &normalized_intersection,
|
|
const IntersectionShape &intersection,
|
|
const std::vector<IntersectionNormalizationOperation> &performed_merges) const
|
|
{
|
|
const auto node_at_intersection = node_based_graph.GetTarget(entering_via_edge);
|
|
|
|
// check if there is a single valid turn entering the current intersection
|
|
const auto only_valid_turn = GetOnlyAllowedTurnIfExistent(previous_node, node_at_intersection);
|
|
|
|
// barriers change our behaviour regarding u-turns
|
|
const bool is_barrier_node = barrier_nodes.find(node_at_intersection) != barrier_nodes.end();
|
|
|
|
const auto connect_to_previous_node = [this, previous_node](const IntersectionShapeData road) {
|
|
return node_based_graph.GetTarget(road.eid) == previous_node;
|
|
};
|
|
|
|
// check which of the edges is the u-turn edge
|
|
const auto uturn_edge_itr =
|
|
std::find_if(intersection.begin(), intersection.end(), connect_to_previous_node);
|
|
|
|
// there needs to be a connection, otherwise stuff went seriously wrong. Note that this is not
|
|
// necessarily the same id as `entering_via_edge`.
|
|
// In cases where parallel edges are present, we only remember the minimal edge. Both share
|
|
// exactly the same coordinates, so the u-turn is still the best choice here.
|
|
BOOST_ASSERT(uturn_edge_itr != intersection.end());
|
|
|
|
const auto is_restricted = [&](const NodeID destination) {
|
|
// check if we have a dedicated destination
|
|
if (only_valid_turn && *only_valid_turn != destination)
|
|
return true;
|
|
|
|
// not explicitly forbidden
|
|
return restriction_map.CheckIfTurnIsRestricted(
|
|
previous_node, node_at_intersection, destination);
|
|
};
|
|
|
|
const auto is_allowed_turn = [&](const IntersectionShapeData &road) {
|
|
const auto &road_data = node_based_graph.GetEdgeData(road.eid);
|
|
const NodeID road_destination_node = node_based_graph.GetTarget(road.eid);
|
|
// reverse edges are never valid turns because the resulting turn would look like this:
|
|
// from_node --via_edge--> node_at_intersection <--onto_edge-- to_node
|
|
// however we need this for capture intersection shape for incoming one-ways
|
|
return !road_data.reversed &&
|
|
// we are not turning over a barrier
|
|
(!is_barrier_node || road_destination_node == previous_node) &&
|
|
// don't allow restricted turns
|
|
!is_restricted(road_destination_node);
|
|
|
|
};
|
|
|
|
// due to merging of roads, the u-turn might actually not be part of the intersection anymore
|
|
const auto uturn_bearing = [&]() {
|
|
const auto merge_entry = std::find_if(
|
|
performed_merges.begin(), performed_merges.end(), [&uturn_edge_itr](const auto entry) {
|
|
return entry.merged_eid == uturn_edge_itr->eid;
|
|
});
|
|
if (merge_entry != performed_merges.end())
|
|
{
|
|
const auto merged_into_id = merge_entry->into_eid;
|
|
const auto merged_u_turn = std::find_if(
|
|
normalized_intersection.begin(),
|
|
normalized_intersection.end(),
|
|
[&](const IntersectionShapeData &road) { return road.eid == merged_into_id; });
|
|
BOOST_ASSERT(merged_u_turn != normalized_intersection.end());
|
|
return util::bearing::reverse(merged_u_turn->bearing);
|
|
}
|
|
else
|
|
{
|
|
const auto uturn_edge_at_normalized_intersection_itr =
|
|
std::find_if(normalized_intersection.begin(),
|
|
normalized_intersection.end(),
|
|
connect_to_previous_node);
|
|
BOOST_ASSERT(uturn_edge_at_normalized_intersection_itr !=
|
|
normalized_intersection.end());
|
|
return util::bearing::reverse(uturn_edge_at_normalized_intersection_itr->bearing);
|
|
}
|
|
}();
|
|
|
|
IntersectionView intersection_view;
|
|
intersection_view.reserve(normalized_intersection.size());
|
|
std::transform(normalized_intersection.begin(),
|
|
normalized_intersection.end(),
|
|
std::back_inserter(intersection_view),
|
|
[&](const IntersectionShapeData &road) {
|
|
return IntersectionViewData(
|
|
road,
|
|
is_allowed_turn(road),
|
|
util::bearing::angleBetween(uturn_bearing, road.bearing));
|
|
});
|
|
|
|
const auto uturn_edge_at_intersection_view_itr =
|
|
std::find_if(intersection_view.begin(), intersection_view.end(), connect_to_previous_node);
|
|
// number of found valid exit roads
|
|
const auto valid_count =
|
|
std::count_if(intersection_view.begin(),
|
|
intersection_view.end(),
|
|
[](const IntersectionViewData &road) { return road.entry_allowed; });
|
|
// in general, we don't wan't to allow u-turns. If we don't look at a barrier, we have to check
|
|
// for dead end streets. These are the only ones that we allow uturns for, next to barriers
|
|
// (which are also kind of a dead end, but we don't have to check these again :))
|
|
if (uturn_edge_at_intersection_view_itr != intersection_view.end() &&
|
|
((uturn_edge_at_intersection_view_itr->entry_allowed && !is_barrier_node &&
|
|
valid_count != 1) ||
|
|
valid_count == 0))
|
|
{
|
|
const auto allow_uturn_at_dead_end = [&]() {
|
|
const auto &uturn_data = node_based_graph.GetEdgeData(uturn_edge_itr->eid);
|
|
|
|
// we can't turn back onto oneway streets
|
|
if (uturn_data.reversed)
|
|
return false;
|
|
|
|
// don't allow explicitly restricted turns
|
|
if (is_restricted(previous_node))
|
|
return false;
|
|
|
|
// we define dead ends as roads that can only be entered via the possible u-turn
|
|
const auto is_bidirectional = [&](const EdgeID entering_via_edge) {
|
|
const auto to_node = node_based_graph.GetTarget(entering_via_edge);
|
|
const auto reverse_edge = node_based_graph.FindEdge(to_node, node_at_intersection);
|
|
BOOST_ASSERT(reverse_edge != SPECIAL_EDGEID);
|
|
return !node_based_graph.GetEdgeData(reverse_edge).reversed;
|
|
};
|
|
|
|
const auto bidirectional_edges = [&]() {
|
|
std::uint32_t count = 0;
|
|
for (const auto eid : node_based_graph.GetAdjacentEdgeRange(node_at_intersection))
|
|
if (is_bidirectional(eid))
|
|
++count;
|
|
return count;
|
|
}();
|
|
|
|
// Checking for dead-end streets is kind of difficult. There is obvious dead ends
|
|
// (single road connected)
|
|
return bidirectional_edges <= 1;
|
|
}();
|
|
uturn_edge_at_intersection_view_itr->entry_allowed = allow_uturn_at_dead_end;
|
|
}
|
|
std::sort(std::begin(intersection_view),
|
|
std::end(intersection_view),
|
|
std::mem_fn(&IntersectionViewData::CompareByAngle));
|
|
|
|
BOOST_ASSERT(intersection_view[0].angle >= 0. &&
|
|
intersection_view[0].angle < std::numeric_limits<double>::epsilon());
|
|
|
|
return intersection_view;
|
|
}
|
|
|
|
boost::optional<NodeID>
|
|
IntersectionGenerator::GetOnlyAllowedTurnIfExistent(const NodeID coming_from_node,
|
|
const NodeID node_at_intersection) const
|
|
{
|
|
// If only restrictions refer to invalid ways somewhere far away, we rather ignore the
|
|
// restriction than to not route over the intersection at all.
|
|
const auto only_restriction_to_node =
|
|
restriction_map.CheckForEmanatingIsOnlyTurn(coming_from_node, node_at_intersection);
|
|
if (only_restriction_to_node != SPECIAL_NODEID)
|
|
{
|
|
// if the mentioned node does not exist anymore, we don't return it. This checks for broken
|
|
// turn restrictions
|
|
for (const auto onto_edge : node_based_graph.GetAdjacentEdgeRange(node_at_intersection))
|
|
if (only_restriction_to_node == node_based_graph.GetTarget(onto_edge))
|
|
return only_restriction_to_node;
|
|
}
|
|
// Ignore broken only restrictions.
|
|
return boost::none;
|
|
}
|
|
|
|
const CoordinateExtractor &IntersectionGenerator::GetCoordinateExtractor() const
|
|
{
|
|
return coordinate_extractor;
|
|
}
|
|
|
|
} // namespace guidance
|
|
} // namespace extractor
|
|
} // namespace osrm
|