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

use anyhow::Result;
use serde::{Deserialize, Serialize};

use abstutil::MultiMap;
use geom::{Angle, Distance, PolyLine, Pt2D};

use crate::raw::RestrictionType;
use crate::{
    DirectedRoadID, Direction, Intersection, IntersectionID, LaneID, Map, PathConstraints,
};

/// Turns are uniquely identified by their (src, dst) lanes and their parent intersection.
/// Intersection is needed to distinguish crosswalks that exist at two ends of a sidewalk.
#[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)]
pub struct TurnID {
    pub parent: IntersectionID,
    /// src and dst must both belong to parent. No guarantees that src is incoming and dst is
    /// outgoing for turns between sidewalks.
    pub src: LaneID,
    pub dst: LaneID,
}

impl fmt::Display for TurnID {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "TurnID({}, {}, {})", self.src, self.dst, self.parent)
    }
}

#[derive(Clone, Copy, Debug, Eq, PartialOrd, Ord, PartialEq, Serialize, Deserialize)]
pub enum TurnType {
    Crosswalk,
    SharedSidewalkCorner,
    // These are for vehicle turns
    Straight,
    Right,
    Left,
    UTurn,
}

// TODO This concept may be dated, now that Movements exist. Within a movement, the lane-changing
// turns should be treated as less important.
#[derive(Serialize, Deserialize, Debug, PartialEq, Clone, Copy, PartialOrd)]
pub enum TurnPriority {
    /// For stop signs: Can't currently specify this!
    /// For traffic signals: Can't do this turn right now.
    Banned,
    /// For stop signs: cars have to stop before doing this turn, and are accepted with the lowest
    /// priority.
    /// For traffic signals: Cars can do this immediately if there are no previously accepted
    /// conflicting turns.
    Yield,
    /// For stop signs: cars can do this without stopping. These can conflict!
    /// For traffic signals: Must be non-conflicting.
    Protected,
}

/// A Turn leads from the end of one Lane to the start of another. (Except for pedestrians;
/// sidewalks are bidirectional.)
#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)]
pub struct Turn {
    pub id: TurnID,
    pub turn_type: TurnType,
    // TODO Some turns might not actually have geometry. Currently encoded by two equal points.
    // Represent more directly?
    pub geom: PolyLine,
    /// Empty except for TurnType::Crosswalk. Usually just one other ID, except for the case of 4
    /// duplicates at a degenerate intersection.
    pub other_crosswalk_ids: BTreeSet<TurnID>,
}

impl Turn {
    pub fn conflicts_with(&self, other: &Turn) -> bool {
        if self.turn_type == TurnType::SharedSidewalkCorner
            || other.turn_type == TurnType::SharedSidewalkCorner
        {
            return false;
        }
        if self.id == other.id {
            return false;
        }
        if self.between_sidewalks() && other.between_sidewalks() {
            return false;
        }

        if self.geom.first_pt() == other.geom.first_pt() {
            return false;
        }
        if self.geom.last_pt() == other.geom.last_pt() {
            return true;
        }
        self.geom.intersection(&other.geom).is_some()
    }

    // TODO What should this be for zero-length turns? Probably src's pt1 to dst's pt2 or
    // something.
    pub fn angle(&self) -> Angle {
        self.geom.first_pt().angle_to(self.geom.last_pt())
    }

    pub fn between_sidewalks(&self) -> bool {
        self.turn_type == TurnType::SharedSidewalkCorner || self.turn_type == TurnType::Crosswalk
    }

    // TODO Maybe precompute this.
    /// Penalties for (lane types, lane-changing, slow lane). The penalty may depend on the vehicle
    /// performing the turn. Lower means preferable.
    pub fn penalty(&self, constraints: PathConstraints, map: &Map) -> (usize, usize, usize) {
        let from = map.get_l(self.id.src);
        let to = map.get_l(self.id.dst);

        // Starting from the farthest from the center line (right in the US), where is this travel
        // lane? Filters by the lane type and ignores lanes that don't go to the target road.
        let from_idx = {
            let mut cnt = 0;
            let r = map.get_r(from.id.road);
            for (l, lt) in r.children(from.dir).iter().rev() {
                if from.lane_type != *lt {
                    continue;
                }
                if map
                    .get_turns_from_lane(*l)
                    .into_iter()
                    .any(|t| t.id.dst.road == to.id.road)
                {
                    cnt += 1;
                    if from.id == *l {
                        break;
                    }
                }
            }
            cnt
        };

        // Starting from the farthest from the center line (right in the US), where is this travel
        // lane? Filters by the lane type.
        let to_idx = {
            let mut cnt = 0;
            let r = map.get_r(to.id.road);
            for (l, lt) in r.children(to.dir).iter().rev() {
                if to.lane_type != *lt {
                    continue;
                }
                cnt += 1;
                if to.id == *l {
                    break;
                }
            }
            cnt
        };

        // TODO I thought about different cases where there are the same/more/less lanes going in
        // and out, but then actually, I think the reasonable thing in all cases is just to do
        // this.
        let lc_cost = ((from_idx as isize) - (to_idx as isize)).abs() as usize;

        // If we're a bike, prefer bike lanes, then bus lanes. If we're a bus, prefer bus lanes.
        // Otherwise, avoid special lanes, even if we're allowed to use them sometimes because they
        // happen to double as turn lanes.
        let lt_cost = if constraints == PathConstraints::Bike {
            if to.is_biking() {
                0
            } else if to.is_bus() {
                1
            } else {
                2
            }
        } else if constraints == PathConstraints::Bus {
            if to.is_bus() {
                0
            } else {
                1
            }
        } else if to.is_bus() {
            // Cars should stay out of bus lanes unless it's required to make a turn
            3
        } else {
            0
        };

        // Keep right (in the US)
        let slow_lane = if to_idx > 1 { 1 } else { 0 };

        (lt_cost, lc_cost, slow_lane)
    }

    pub fn is_crossing_arterial_intersection(&self, map: &Map) -> bool {
        use crate::osm::RoadRank;
        if self.turn_type != TurnType::Crosswalk {
            return false;
        }
        // Distance-only metric has many false positives and negatives
        // return turn.geom.length() > Distance::feet(41.0);

        let intersection = map.get_i(self.id.parent);
        intersection.roads.iter().any(|r| {
            let rank = map.get_r(*r).get_rank();
            rank == RoadRank::Arterial || rank == RoadRank::Highway
        })
    }

    /// Is this turn legal, according to turn lane tagging?
    pub(crate) fn permitted_by_lane(&self, map: &Map) -> bool {
        if let Some(types) = map
            .get_l(self.id.src)
            .get_lane_level_turn_restrictions(map.get_parent(self.id.src), false)
        {
            types.contains(&self.turn_type)
        } else {
            true
        }
    }

    /// Is this turn legal, according to turn restrictions defined between road segments?
    pub(crate) fn permitted_by_road(&self, i: &Intersection, map: &Map) -> bool {
        if self.between_sidewalks() {
            return true;
        }

        let src = map.get_parent(self.id.src);
        let dst = self.id.dst.road;

        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
    }
}

/// A movement is like a turn, but with less detail -- it identifies a movement from one directed
/// road to another.
/// One road usually has 4 crosswalks, each a singleton Movement. We need all of the information
/// here to keep each crosswalk separate.
#[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)]
pub struct MovementID {
    pub from: DirectedRoadID,
    pub to: DirectedRoadID,
    pub parent: IntersectionID,
    pub crosswalk: bool,
}

impl MovementID {
    // TODO Expensive! Should we natively store movements everywhere?
    pub(crate) fn get(self, map: &Map) -> Result<Movement> {
        Ok(Movement::for_i(self.parent, map)?.remove(&self).unwrap())
    }
}

/// This is cheaper to store than a MovementID. It simply indexes into the list of movements.
#[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)]
pub struct CompressedMovementID {
    pub i: IntersectionID,
    // There better not be any intersection with more than 256 movements...
    pub idx: u8,
}

/// A Movement groups all turns from one road to another, letting traffic signals operate at a
/// higher level of abstraction.
/// This is used for pathfinding and traffic signals currently; other places focus instead on turns.
// TODO Unclear how this plays with different lane types
#[derive(Debug, Serialize, Deserialize, Clone, PartialEq)]
pub struct Movement {
    pub id: MovementID,
    pub turn_type: TurnType,
    pub members: Vec<TurnID>,
    /// The "overall" path of movement, aka, an "average" of the turn geometry
    pub geom: PolyLine,
    pub angle: Angle,
}

impl Movement {
    pub(crate) fn for_i(i: IntersectionID, map: &Map) -> Result<BTreeMap<MovementID, Movement>> {
        let mut results = BTreeMap::new();
        let mut movements: MultiMap<(DirectedRoadID, DirectedRoadID), TurnID> = MultiMap::new();
        for turn in map.get_turns_in_intersection(i) {
            let from = map.get_l(turn.id.src).get_directed_parent();
            let to = map.get_l(turn.id.dst).get_directed_parent();
            match turn.turn_type {
                TurnType::SharedSidewalkCorner => {}
                TurnType::Crosswalk => {
                    let id = MovementID {
                        from,
                        to,
                        parent: i,
                        crosswalk: true,
                    };
                    results.insert(
                        id,
                        Movement {
                            id,
                            turn_type: TurnType::Crosswalk,
                            members: vec![turn.id],
                            geom: turn.geom.clone(),
                            angle: turn.angle(),
                        },
                    );
                }
                _ => {
                    movements.insert((from, to), turn.id);
                }
            }
        }
        for ((from, to), members) in movements.consume() {
            let geom = movement_geom(
                members.iter().map(|t| &map.get_t(*t).geom).collect(),
                from,
                to,
            )?;
            let turn_types: BTreeSet<TurnType> =
                members.iter().map(|t| map.get_t(*t).turn_type).collect();
            if turn_types.len() > 1 {
                warn!(
                    "Movement between {} and {} has weird turn types! {:?}",
                    from, to, turn_types
                );
            }
            let members: Vec<TurnID> = members.into_iter().collect();
            let id = MovementID {
                from,
                to,
                parent: i,
                crosswalk: false,
            };
            results.insert(
                id,
                Movement {
                    id,
                    turn_type: *turn_types.iter().next().unwrap(),
                    angle: map.get_t(members[0]).angle(),
                    members,
                    geom,
                },
            );
        }
        if results.is_empty() {
            bail!("No Movements! Does the intersection have at least 2 roads?");
        }
        Ok(results)
    }

    /// Polyline points FROM intersection
    pub fn src_center_and_width(&self, map: &Map) -> (PolyLine, Distance) {
        let r = map.get_r(self.id.from.id);

        let mut leftmost = Distance::meters(99999.0);
        let mut rightmost = Distance::ZERO;
        let mut left = Distance::ZERO;

        for l in &r.lanes {
            let right = left + l.width;

            if self.members.iter().any(|t| t.src == l.id) {
                leftmost = leftmost.min(left);
                rightmost = rightmost.max(right);
            }

            left = right;
        }

        let mut pl = r
            .get_left_side()
            .must_shift_right((leftmost + rightmost) / 2.0);
        if self.id.from.dir == Direction::Back {
            pl = pl.reversed();
        }
        // Flip direction, so we point away from the intersection
        if !self.id.crosswalk || map.get_l(self.members[0].src).src_i != self.members[0].parent {
            pl = pl.reversed()
        };
        (pl, rightmost - leftmost)
    }

    pub fn conflicts_with(&self, other: &Movement) -> bool {
        if self.id == other.id {
            return false;
        }
        if self.turn_type == TurnType::Crosswalk && other.turn_type == TurnType::Crosswalk {
            return false;
        }

        if self.id.from == other.id.from
            && self.turn_type != TurnType::Crosswalk
            && other.turn_type != TurnType::Crosswalk
        {
            return false;
        }
        if self.id.to == other.id.to
            && self.turn_type != TurnType::Crosswalk
            && other.turn_type != TurnType::Crosswalk
        {
            return true;
        }
        // TODO If you hit a panic below, you've probably got two separate roads overlapping.
        // Fix it in OSM. Examples: https://www.openstreetmap.org/changeset/87465499,
        // https://www.openstreetmap.org/changeset/85952811
        /*if self.geom == other.geom {
            println!("*********** {:?} and {:?} have the same geom", self.id, other.id);
            return true;
        }*/
        self.geom.intersection(&other.geom).is_some()
    }
}

fn movement_geom(
    polylines: Vec<&PolyLine>,
    from: DirectedRoadID,
    to: DirectedRoadID,
) -> Result<PolyLine> {
    let num_pts = polylines[0].points().len();
    for pl in &polylines {
        if num_pts != pl.points().len() {
            // Kiiiiinda spammy
            if false {
                warn!(
                    "Movement between {} and {} can't make nice geometry",
                    from, to
                );
            }
            return Ok(polylines[0].clone());
        }
    }

    let mut pts = Vec::new();
    for idx in 0..num_pts {
        pts.push(Pt2D::center(
            &polylines
                .iter()
                .map(|pl| pl.points()[idx])
                .collect::<Vec<_>>(),
        ));
    }
    PolyLine::deduping_new(pts)
}

impl TurnID {
    pub fn to_movement(self, map: &Map) -> MovementID {
        MovementID {
            from: map.get_l(self.src).get_directed_parent(),
            to: map.get_l(self.dst).get_directed_parent(),
            parent: self.parent,
            crosswalk: map.get_l(self.src).is_walkable(),
        }
    }
}