-
Notifications
You must be signed in to change notification settings - Fork 3.3k
/
intersection_generator.cpp
488 lines (428 loc) · 22.4 KB
/
intersection_generator.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
#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 EdgeBasedNodeDataContainer &node_data_container,
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), node_data_container(node_data_container),
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)
.flags.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;
// check if there is a relevant change in the graph
if (!CanBeCompressed(node_based_graph.GetEdgeData(query_edge),
node_based_graph.GetEdgeData(next_edge),
node_data_container) ||
(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