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
use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet};

use anyhow::Result;
use nbez::{Bez3o, BezCurve, Point2d};

use geom::{Angle, Distance, Line, PolyLine, Pt2D};

use crate::raw::RestrictionType;
use crate::{Intersection, Lane, LaneID, Map, RoadID, Turn, TurnID, TurnType};

/// Generate all driving and walking turns at an intersection, accounting for OSM turn restrictions.
pub fn make_all_turns(map: &Map, i: &Intersection) -> Vec<Turn> {
    let mut raw_turns: Vec<Turn> = Vec::new();
    raw_turns.extend(make_vehicle_turns(i, map));
    raw_turns.extend(crate::make::walking_turns::filter_turns(
        crate::make::walking_turns::make_walking_turns(map, i),
        map,
        i,
    ));
    let unique_turns = ensure_unique(raw_turns);

    let mut final_turns: Vec<Turn> = Vec::new();
    let mut filtered_turns: HashMap<LaneID, Vec<Turn>> = HashMap::new();
    for turn in unique_turns {
        if !does_turn_pass_restrictions(&turn, i, map) {
            continue;
        }

        if is_turn_allowed(&turn, map) {
            final_turns.push(turn);
        } else {
            filtered_turns
                .entry(turn.id.src)
                .or_insert_with(Vec::new)
                .push(turn);
        }
    }

    // Make sure every incoming lane has a turn originating from it, and every outgoing lane has a
    // turn leading to it.
    let mut incoming_missing: HashSet<LaneID> = HashSet::new();
    for l in &i.incoming_lanes {
        if map.get_l(*l).lane_type.supports_any_movement() {
            incoming_missing.insert(*l);
        }
    }
    for t in &final_turns {
        incoming_missing.remove(&t.id.src);
    }
    for (l, turns) in filtered_turns {
        // Do turn restrictions orphan a lane?
        if incoming_missing.contains(&l) {
            // Restrictions on turn lanes may sometimes actually be more like change:lanes
            // (https://wiki.openstreetmap.org/wiki/Key:change). Try to interpret them that way
            // here, choosing one turn from a bunch of options.

            // If all the turns go to a single road, then ignore the turn type.
            let dst_r = map.get_l(turns[0].id.dst).parent;
            let single_group: Vec<Turn> =
                if turns.iter().all(|t| map.get_l(t.id.dst).parent == dst_r) {
                    turns.clone()
                } else {
                    // Fall back to preferring all the straight turns
                    turns
                        .iter()
                        .filter(|t| t.turn_type == TurnType::Straight)
                        .cloned()
                        .collect()
                };
            if !single_group.is_empty() {
                // Just pick one, with the lowest lane-changing cost. Not using Turn's penalty()
                // here, because
                // 1) We haven't populated turns yet, so from_idx won't work
                // 2) It counts from the right, but I think we actually want to count from the left
                let best = single_group
                    .into_iter()
                    .min_by_key(|t| lc_penalty(t, map))
                    .unwrap();
                final_turns.push(best);
                info!(
                    "Restricted lane-changing on approach to turn lanes at {}",
                    l
                );
            } else {
                warn!("Turn restrictions broke {} outbound, so restoring turns", l);
                final_turns.extend(turns);
            }
            incoming_missing.remove(&l);
        }
    }

    final_turns = remove_merging_turns(map, final_turns, TurnType::Right);
    final_turns = remove_merging_turns(map, final_turns, TurnType::Left);

    if i.merged {
        final_turns.retain(|turn| {
            if turn.turn_type == TurnType::UTurn {
                warn!("Removing u-turn from merged intersection: {}", turn.id);
                false
            } else {
                true
            }
        });
    }

    let mut outgoing_missing: HashSet<LaneID> = HashSet::new();
    for l in &i.outgoing_lanes {
        if map.get_l(*l).lane_type.supports_any_movement() {
            outgoing_missing.insert(*l);
        }
    }
    for t in &final_turns {
        outgoing_missing.remove(&t.id.dst);
    }

    if !incoming_missing.is_empty() || !outgoing_missing.is_empty() {
        warn!(
            "Turns for {} orphan some lanes. Incoming: {:?}, outgoing: {:?}",
            i.id, incoming_missing, outgoing_missing
        );
    }

    final_turns
}

fn ensure_unique(turns: Vec<Turn>) -> Vec<Turn> {
    let mut ids = HashSet::new();
    let mut keep: Vec<Turn> = Vec::new();
    for t in turns.into_iter() {
        if ids.contains(&t.id) {
            // TODO This was once an assertion, but disabled for
            // https://github.com/a-b-street/abstreet/issues/84. A crosswalk gets created twice
            // and deduplicated here. Not sure why it was double-created in the first place.
            warn!("Duplicate turns {}!", t.id);
        } else {
            ids.insert(t.id);
            keep.push(t);
        }
    }
    keep
}

fn is_turn_allowed(turn: &Turn, map: &Map) -> bool {
    if let Some(types) = map
        .get_l(turn.id.src)
        .get_turn_restrictions(map.get_parent(turn.id.src))
    {
        types.contains(&turn.turn_type)
    } else {
        true
    }
}

fn does_turn_pass_restrictions(turn: &Turn, i: &Intersection, map: &Map) -> bool {
    if turn.between_sidewalks() {
        return true;
    }

    let src = map.get_parent(turn.id.src);
    let dst = map.get_l(turn.id.dst).parent;

    for (restriction, to) in &src.turn_restrictions {
        // The restriction only applies to one direction of the road.
        if !i.roads.contains(to) {
            continue;
        }
        match restriction {
            RestrictionType::BanTurns => {
                if dst == *to {
                    return false;
                }
            }
            RestrictionType::OnlyAllowTurns => {
                if dst != *to {
                    return false;
                }
            }
        }
    }

    true
}

fn make_vehicle_turns(i: &Intersection, map: &Map) -> Vec<Turn> {
    let mut turns = Vec::new();

    let expected_turn_types = expected_turn_types_for_four_way(i, map);

    // Just generate every possible combination of turns between incoming and outgoing lanes.
    let is_deadend = i.roads.len() == 1;
    for src in &i.incoming_lanes {
        let src = map.get_l(*src);
        if !src.lane_type.is_for_moving_vehicles() {
            continue;
        }
        for dst in &i.outgoing_lanes {
            let dst = map.get_l(*dst);
            if !dst.lane_type.is_for_moving_vehicles() {
                continue;
            }
            // Only allow U-turns at deadends
            if src.parent == dst.parent && !is_deadend {
                continue;
            }
            // Can't go between light rail and normal roads
            if src.is_light_rail() != dst.is_light_rail() {
                continue;
            }
            if src.last_pt() == dst.first_pt() {
                warn!(
                    "No turn from {} to {}; the endpoints are the same",
                    src.id, dst.id
                );
                continue;
            }

            let from_angle = src.last_line().angle();
            let to_angle = dst.first_line().angle();
            let mut turn_type = turn_type_from_angles(from_angle, to_angle);
            if turn_type == TurnType::UTurn {
                // Lots of false positives when classifying these just based on angles. So also
                // require the road names to match.
                if map.get_parent(src.id).get_name(None) != map.get_parent(dst.id).get_name(None) {
                    // Distinguish really sharp lefts/rights based on clockwiseness
                    if from_angle.simple_shortest_rotation_towards(to_angle) < 0.0 {
                        turn_type = TurnType::Right;
                    } else {
                        turn_type = TurnType::Left;
                    }
                }

                // Some service roads wind up very short. Allowing u-turns there causes vehicles to
                // gridlock pretty much instantly, because they occupy two intersections during the
                // attempted movement.
                if is_deadend && src.length() < Distance::meters(7.0) {
                    warn!("Skipping U-turn at tiny deadend on {}", src.id);
                    continue;
                }
            } else if let Some(expected_type) = expected_turn_types
                .as_ref()
                .and_then(|e| e.get(&(src.parent, dst.parent)))
            {
                // At some 4-way intersections, roads meet at strange angles, throwing off
                // turn_type_from_angles. Correct it based on relative ordering.
                if turn_type != *expected_type {
                    warn!(
                        "Turn from {} to {} looks like {:?} by angle, but is {:?} by ordering",
                        src.parent, dst.parent, turn_type, expected_type
                    );
                    turn_type = *expected_type;
                }
            }

            let geom = if turn_type == TurnType::Straight {
                PolyLine::must_new(vec![src.last_pt(), dst.first_pt()])
            } else {
                curvey_turn(src, dst)
                    .unwrap_or_else(|_| PolyLine::must_new(vec![src.last_pt(), dst.first_pt()]))
            };

            turns.push(Turn {
                id: TurnID {
                    parent: i.id,
                    src: src.id,
                    dst: dst.id,
                },
                turn_type,
                other_crosswalk_ids: BTreeSet::new(),
                geom,
            });
        }
    }

    turns
}

fn curvey_turn(src: &Lane, dst: &Lane) -> Result<PolyLine> {
    // The control points are straight out/in from the source/destination lanes, so
    // that the car exits and enters at the same angle as the road.
    let src_line = src.last_line();
    let dst_line = dst.first_line().reverse();

    // TODO Tune the 5.0 and pieces
    let pt1 = src.last_pt();
    let control_pt1 = src_line.unbounded_dist_along(src_line.length() + Distance::meters(5.0));
    let control_pt2 = dst_line.unbounded_dist_along(dst_line.length() + Distance::meters(5.0));
    let pt2 = dst.first_pt();

    // If the intersection is too small, the endpoints and control points squish together, and
    // they'll overlap. In that case, just use the straight line for the turn.
    if let (Some(l1), Some(l2)) = (Line::new(pt1, control_pt1), Line::new(control_pt2, pt2)) {
        if l1.crosses(&l2) {
            // TODO Make this bail!, so we use the straight line. But it gridlocked two maps, so
            // resolve that first.
            warn!("intersection is too small for a Bezier curve");
        }
    }

    let curve = Bez3o::new(
        to_pt(pt1),
        to_pt(control_pt1),
        to_pt(control_pt2),
        to_pt(pt2),
    );
    let pieces = 5;
    let mut curve: Vec<Pt2D> = (0..=pieces)
        .map(|i| {
            from_pt(
                curve
                    .interp(1.0 / f64::from(pieces) * f64::from(i))
                    .unwrap(),
            )
        })
        .collect();
    curve.dedup();
    PolyLine::new(curve)
}

fn to_pt(pt: Pt2D) -> Point2d<f64> {
    Point2d::new(pt.x(), pt.y())
}

fn from_pt(pt: Point2d<f64>) -> Pt2D {
    Pt2D::new(pt.x, pt.y)
}

fn lc_penalty(t: &Turn, map: &Map) -> isize {
    let from = map.get_l(t.id.src);
    let to = map.get_l(t.id.dst);

    let from_idx = {
        let mut cnt = 0;
        let r = map.get_r(from.parent);
        for (l, lt) in r.children(r.dir(from.id)) {
            if from.lane_type != lt {
                continue;
            }
            cnt += 1;
            if from.id == l {
                break;
            }
        }
        cnt
    };

    let to_idx = {
        let mut cnt = 0;
        let r = map.get_r(to.parent);
        for (l, lt) in r.children(r.dir(to.id)) {
            if to.lane_type != lt {
                continue;
            }
            cnt += 1;
            if to.id == l {
                break;
            }
        }
        cnt
    };

    ((from_idx as isize) - (to_idx as isize)).abs()
}

fn remove_merging_turns(map: &Map, input: Vec<Turn>, turn_type: TurnType) -> Vec<Turn> {
    let mut turns = Vec::new();

    // Group turns of the specified type by (from, to) road
    let mut pairs: BTreeMap<(RoadID, RoadID), Vec<Turn>> = BTreeMap::new();
    for t in input {
        // Only do this for driving lanes
        if !map.get_l(t.id.src).is_driving() || !map.get_l(t.id.dst).is_driving() {
            turns.push(t);
            continue;
        }

        if t.turn_type == turn_type {
            pairs
                .entry((map.get_l(t.id.src).parent, map.get_l(t.id.dst).parent))
                .or_insert_with(Vec::new)
                .push(t);
        } else {
            turns.push(t);
        }
    }

    for (_, group) in pairs {
        if group.len() == 1 {
            turns.extend(group);
            continue;
        }

        // From one to many is fine
        if group.iter().map(|t| t.id.src).collect::<HashSet<_>>().len() == 1 {
            turns.extend(group);
            continue;
        }

        // We have multiple lanes all with a turn to the same destination road. Most likely, only
        // the rightmost or leftmost can actually make the turn.
        // TODO If OSM turn restrictions explicitly have something like "left|left|", then there
        // are multiple source lanes!
        let road = map.get_parent(group[0].id.src);
        let src = if turn_type == TurnType::Right {
            group
                .iter()
                .max_by_key(|t| road.dir_and_offset(t.id.src).1)
                .unwrap()
                .id
                .src
        } else if turn_type == TurnType::Left {
            group
                .iter()
                .min_by_key(|t| road.dir_and_offset(t.id.src).1)
                .unwrap()
                .id
                .src
        } else {
            unreachable!()
        };
        for t in group {
            if t.id.src == src {
                turns.push(t);
            }
        }

        // That left or rightmost lane can turn into all lanes on the destination road. Tempting to
        // remove this, but it may remove some valid U-turn movements (like on Mercer).
    }
    turns
}

fn turn_type_from_angles(from: Angle, to: Angle) -> TurnType {
    let diff = from.simple_shortest_rotation_towards(to);
    // This is a pretty arbitrary parameter, but a difference of 30 degrees seems reasonable for
    // some observed cases.
    if diff.abs() < 30.0 {
        TurnType::Straight
    } else if diff.abs() > 135.0 {
        TurnType::UTurn
    } else if diff < 0.0 {
        // Clockwise rotation
        TurnType::Right
    } else {
        // Counter-clockwise rotation
        TurnType::Left
    }
}

fn expected_turn_types_for_four_way(
    i: &Intersection,
    map: &Map,
) -> Option<HashMap<(RoadID, RoadID), TurnType>> {
    let roads = i.get_sorted_incoming_roads(map);
    if roads.len() != 4 {
        return None;
    }

    // Just based on relative ordering around the intersection, turns (from road, to road, should
    // have this type)
    let mut expected_turn_types: HashMap<(RoadID, RoadID), TurnType> = HashMap::new();
    for (offset, turn_type) in vec![
        (1, TurnType::Left),
        (2, TurnType::Straight),
        (3, TurnType::Right),
    ] {
        for from_idx in 0..roads.len() {
            let to = *abstutil::wraparound_get(&roads, (from_idx as isize) + offset);
            expected_turn_types.insert((roads[from_idx], to), turn_type);
        }
    }
    Some(expected_turn_types)
}