Michael Krasnyk
486 lines
22 KiB
C++
486 lines
22 KiB
C++
#include "extractor/guidance/intersection_generator.hpp"
|
|
|
|
#include "extractor/geojson_debug_policies.hpp"
|
|
|
|
#include "util/geojson_debug_logger.hpp"
|
|
|
|
#include "util/assert.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<util::Coordinate> &coordinates,
|
|
const CompressedEdgeContainer &compressed_edge_container)
|
|
: node_based_graph(node_based_graph), restriction_map(restriction_map),
|
|
barrier_nodes(barrier_nodes), coordinates(coordinates),
|
|
coordinate_extractor(node_based_graph, compressed_edge_container, coordinates)
|
|
{
|
|
}
|
|
|
|
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
|
|
{
|
|
const auto intersection_degree = node_based_graph.GetOutDegree(node_at_center_of_intersection);
|
|
const util::Coordinate turn_coordinate = coordinates[node_at_center_of_intersection];
|
|
|
|
// compute bearings in a relatively small circle to prevent wrong roads order with true bearings
|
|
struct RoadWithInitialBearing
|
|
{
|
|
double bearing;
|
|
IntersectionShapeData road;
|
|
};
|
|
std::vector<RoadWithInitialBearing> initial_roads_ordering;
|
|
// reserve enough items (+ the possibly missing u-turn edge)
|
|
initial_roads_ordering.reserve(intersection_degree);
|
|
|
|
// 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 close_coordinate =
|
|
coordinate_extractor.ExtractCoordinateAtLength(2. /*m*/, coordinates);
|
|
const auto initial_bearing =
|
|
util::coordinate_calculation::bearing(turn_coordinate, close_coordinate);
|
|
|
|
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);
|
|
}
|
|
|
|
initial_roads_ordering.push_back(
|
|
{initial_bearing, {edge_connected_to_intersection, bearing, segment_length}});
|
|
}
|
|
|
|
if (!initial_roads_ordering.empty())
|
|
{
|
|
const auto base_initial_bearing = [&]() {
|
|
if (sorting_base)
|
|
{
|
|
const auto itr = std::find_if(initial_roads_ordering.begin(),
|
|
initial_roads_ordering.end(),
|
|
[&](const auto &data) {
|
|
return node_based_graph.GetTarget(
|
|
data.road.eid) == *sorting_base;
|
|
});
|
|
if (itr != initial_roads_ordering.end())
|
|
return util::bearing::reverse(itr->bearing);
|
|
}
|
|
return util::bearing::reverse(initial_roads_ordering.begin()->bearing);
|
|
}();
|
|
|
|
// sort roads with respect to the initial bearings, a tie-breaker for equal initial bearings
|
|
// is to order roads via final bearings to have roads in clockwise order
|
|
//
|
|
// rhs <---. lhs <----.
|
|
// / /
|
|
// lhs / rhs /
|
|
//
|
|
// lhs road is before rhs one rhs road is before lhs one
|
|
// bearing::angleBetween < 180 bearing::angleBetween > 180
|
|
const auto initial_bearing_order = makeCompareShapeDataAngleToBearing(base_initial_bearing);
|
|
std::sort(initial_roads_ordering.begin(),
|
|
initial_roads_ordering.end(),
|
|
[&initial_bearing_order](const auto &lhs, const auto &rhs) {
|
|
return initial_bearing_order(lhs, rhs) ||
|
|
(lhs.bearing == rhs.bearing &&
|
|
util::bearing::angleBetween(lhs.road.bearing, rhs.road.bearing) <
|
|
180);
|
|
});
|
|
|
|
// copy intersection data in the initial order
|
|
IntersectionShape intersection;
|
|
intersection.reserve(initial_roads_ordering.size());
|
|
std::transform(initial_roads_ordering.begin(),
|
|
initial_roads_ordering.end(),
|
|
std::back_inserter(intersection),
|
|
[](const auto &entry) { return entry.road; });
|
|
|
|
if (intersection.size() > 2)
|
|
{ // Check bearings ordering with respect to true bearings
|
|
const auto base_bearing = intersection.front().bearing;
|
|
const auto bearings_order =
|
|
makeCompareShapeDataAngleToBearing(util::bearing::reverse(base_bearing));
|
|
for (auto curr = intersection.begin(), next = std::next(curr);
|
|
next != intersection.end();
|
|
++curr, ++next)
|
|
{
|
|
if (bearings_order(*next, *curr))
|
|
{ // If the true bearing is out of the initial order (next before current) then
|
|
// adjust the next bearing to keep the order. The adjustment angle is at most
|
|
// 0.5° or a half-angle between the current bearing and the base bearing.
|
|
// to prevent overlapping over base bearing + 360°.
|
|
const auto angle_adjustment = std::min(
|
|
.5, util::restrictAngleToValidRange(base_bearing - curr->bearing) / 2.);
|
|
next->bearing =
|
|
util::restrictAngleToValidRange(curr->bearing + angle_adjustment);
|
|
}
|
|
}
|
|
}
|
|
return intersection;
|
|
}
|
|
|
|
return IntersectionShape{};
|
|
}
|
|
|
|
// 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);
|
|
|
|
// request all turn restrictions
|
|
auto const restrictions = restriction_map.Restrictions(previous_node, node_at_intersection);
|
|
|
|
// check turn restrictions to find a node that is the only allowed target when coming from a
|
|
// node to an intersection
|
|
// d
|
|
// |
|
|
// a - b - c and `only_straight_on ab | bc would return `c` for `a,b`
|
|
const auto find_only_valid_turn = [&]() -> boost::optional<NodeID> {
|
|
const auto itr = std::find_if(restrictions.first, restrictions.second, [](auto pair) {
|
|
return pair.second->is_only;
|
|
});
|
|
if (itr != restrictions.second)
|
|
return itr->second->AsNodeRestriction().to;
|
|
else
|
|
return boost::none;
|
|
};
|
|
|
|
const auto only_valid_turn = find_only_valid_turn();
|
|
|
|
// 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)
|
|
return *only_valid_turn != destination;
|
|
|
|
// check if explicitly forbidden
|
|
return restrictions.second !=
|
|
std::find_if(restrictions.first, restrictions.second, [&](const auto &restriction) {
|
|
return restriction.second->AsNodeRestriction().to == 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
|
|
// uturn is a pair of {edge id, bearing}
|
|
const auto uturn = [&]() {
|
|
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 std::make_pair(merged_u_turn->eid,
|
|
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 std::make_pair(
|
|
uturn_edge_at_normalized_intersection_itr->eid,
|
|
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.second, 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));
|
|
|
|
// Move entering_via_edge to intersection front and place all roads prior entering_via_edge
|
|
// at the end of the intersection view with 360° angle
|
|
auto entering_via_it = std::find_if(intersection_view.begin(),
|
|
intersection_view.end(),
|
|
[&uturn](auto &road) { return road.eid == uturn.first; });
|
|
|
|
OSRM_ASSERT(entering_via_it != intersection_view.end() && entering_via_it->angle >= 0. &&
|
|
entering_via_it->angle < std::numeric_limits<double>::epsilon(),
|
|
coordinates[node_at_intersection]);
|
|
|
|
if (entering_via_it != intersection_view.begin() && entering_via_it != intersection_view.end())
|
|
{
|
|
std::for_each(
|
|
intersection_view.begin(), entering_via_it, [](auto &road) { road.angle = 360.; });
|
|
std::rotate(intersection_view.begin(), entering_via_it, intersection_view.end());
|
|
}
|
|
|
|
return intersection_view;
|
|
}
|
|
|
|
const CoordinateExtractor &IntersectionGenerator::GetCoordinateExtractor() const
|
|
{
|
|
return coordinate_extractor;
|
|
}
|
|
|
|
} // namespace guidance
|
|
} // namespace extractor
|
|
} // namespace osrm
|