1 mod ch;
2 mod dijkstra;
3 mod driving;
4 mod node_map;
5 // TODO tmp
6 pub mod uber_turns;
7 mod walking;
8
9 pub use self::ch::ContractionHierarchyPathfinder;
10 pub use self::driving::driving_cost;
11 pub use self::walking::{walking_cost, WalkingNode};
12 use crate::{
13 osm, BusRouteID, BusStopID, Lane, LaneID, LaneType, Map, Position, Traversable, TurnID,
14 UberTurn,
15 };
16 use abstutil::Timer;
17 use enumset::EnumSetType;
18 use geom::{Distance, PolyLine, EPSILON_DIST};
19 use serde::{Deserialize, Serialize};
20 use std::collections::VecDeque;
21 use std::fmt;
22
23 #[derive(Debug, Clone, Copy, Serialize, Deserialize, PartialEq, Eq, Hash, PartialOrd, Ord)]
24 pub enum PathStep {
25 // Original direction
26 Lane(LaneID),
27 // Sidewalks only!
28 ContraflowLane(LaneID),
29 Turn(TurnID),
30 }
31
32 impl PathStep {
as_traversable(&self) -> Traversable33 pub fn as_traversable(&self) -> Traversable {
34 match self {
35 PathStep::Lane(id) => Traversable::Lane(*id),
36 PathStep::ContraflowLane(id) => Traversable::Lane(*id),
37 PathStep::Turn(id) => Traversable::Turn(*id),
38 }
39 }
40
as_lane(&self) -> LaneID41 pub fn as_lane(&self) -> LaneID {
42 self.as_traversable().as_lane()
43 }
44
as_turn(&self) -> TurnID45 pub fn as_turn(&self) -> TurnID {
46 self.as_traversable().as_turn()
47 }
48
49 // Returns dist_remaining. start is relative to the start of the actual geometry -- so from the
50 // lane's real start for ContraflowLane.
slice( &self, map: &Map, start: Distance, dist_ahead: Option<Distance>, ) -> Result<(PolyLine, Distance), String>51 fn slice(
52 &self,
53 map: &Map,
54 start: Distance,
55 dist_ahead: Option<Distance>,
56 ) -> Result<(PolyLine, Distance), String> {
57 if let Some(d) = dist_ahead {
58 if d < Distance::ZERO {
59 panic!("Negative dist_ahead?! {}", d);
60 }
61 if d == Distance::ZERO {
62 return Err(format!("0 dist ahead for slice"));
63 }
64 }
65
66 match self {
67 PathStep::Lane(id) => {
68 let pts = &map.get_l(*id).lane_center_pts;
69 if let Some(d) = dist_ahead {
70 pts.slice(start, start + d)
71 } else {
72 pts.slice(start, pts.length())
73 }
74 }
75 PathStep::ContraflowLane(id) => {
76 let pts = map.get_l(*id).lane_center_pts.reversed();
77 let reversed_start = pts.length() - start;
78 if let Some(d) = dist_ahead {
79 pts.slice(reversed_start, reversed_start + d)
80 } else {
81 pts.slice(reversed_start, pts.length())
82 }
83 }
84 PathStep::Turn(id) => {
85 let pts = &map.get_t(*id).geom;
86 if let Some(d) = dist_ahead {
87 pts.slice(start, start + d)
88 } else {
89 pts.slice(start, pts.length())
90 }
91 }
92 }
93 }
94 }
95
96 #[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
97 pub struct Path {
98 steps: VecDeque<PathStep>,
99 end_dist: Distance,
100
101 // Also track progress along the original path.
102 total_length: Distance,
103 crossed_so_far: Distance,
104
105 total_lanes: usize,
106
107 // A list of uber-turns encountered by this path, in order. The steps are flattened into the
108 // sequence of turn->lane->...->turn.
109 uber_turns: VecDeque<UberTurn>,
110 // Is the current_step in the middle of an UberTurn?
111 currently_inside_ut: Option<UberTurn>,
112 }
113
114 impl Path {
new( map: &Map, steps: Vec<PathStep>, end_dist: Distance, uber_turns: Vec<UberTurn>, ) -> Path115 pub(crate) fn new(
116 map: &Map,
117 steps: Vec<PathStep>,
118 end_dist: Distance,
119 uber_turns: Vec<UberTurn>,
120 ) -> Path {
121 // Haven't seen problems here in a very long time. Noticeably saves some time to skip.
122 if false {
123 validate_continuity(map, &steps);
124 }
125 if false {
126 validate_restrictions(map, &steps);
127 }
128 // Slightly expensive, but the contraction hierarchy weights aren't distances.
129 let mut total_length = Distance::ZERO;
130 let mut total_lanes = 0;
131 for s in &steps {
132 total_length += s.as_traversable().length(map);
133 match s {
134 PathStep::Lane(_) | PathStep::ContraflowLane(_) => total_lanes += 1,
135 _ => {}
136 }
137 }
138 Path {
139 steps: VecDeque::from(steps),
140 end_dist,
141 total_length,
142 crossed_so_far: Distance::ZERO,
143 total_lanes,
144 uber_turns: uber_turns.into_iter().collect(),
145 currently_inside_ut: None,
146 }
147 }
148
one_step(l: LaneID, map: &Map) -> Path149 pub fn one_step(l: LaneID, map: &Map) -> Path {
150 Path::new(
151 map,
152 vec![PathStep::Lane(l)],
153 map.get_l(l).length(),
154 Vec::new(),
155 )
156 }
157
158 // Only used for weird serialization magic.
dummy() -> Path159 pub fn dummy() -> Path {
160 Path {
161 steps: VecDeque::new(),
162 end_dist: Distance::ZERO,
163 total_length: Distance::ZERO,
164 crossed_so_far: Distance::ZERO,
165 total_lanes: 0,
166 uber_turns: VecDeque::new(),
167 currently_inside_ut: None,
168 }
169 }
170
total_lanes(&self) -> usize171 pub fn total_lanes(&self) -> usize {
172 self.total_lanes
173 }
lanes_crossed_so_far(&self) -> usize174 pub fn lanes_crossed_so_far(&self) -> usize {
175 let mut remaining = 0;
176 for s in &self.steps {
177 match s {
178 PathStep::Lane(_) | PathStep::ContraflowLane(_) => remaining += 1,
179 _ => {}
180 };
181 }
182 self.total_lanes - remaining
183 }
184
crossed_so_far(&self) -> Distance185 pub fn crossed_so_far(&self) -> Distance {
186 self.crossed_so_far
187 }
188
total_length(&self) -> Distance189 pub fn total_length(&self) -> Distance {
190 self.total_length
191 }
192
percent_dist_crossed(&self) -> f64193 pub fn percent_dist_crossed(&self) -> f64 {
194 // Sometimes this happens
195 if self.total_length == Distance::ZERO {
196 return 1.0;
197 }
198 self.crossed_so_far / self.total_length
199 }
200
is_empty(&self) -> bool201 pub fn is_empty(&self) -> bool {
202 self.steps.is_empty()
203 }
204
is_last_step(&self) -> bool205 pub fn is_last_step(&self) -> bool {
206 self.steps.len() == 1
207 }
208
isnt_last_step(&self) -> bool209 pub fn isnt_last_step(&self) -> bool {
210 self.steps.len() > 1
211 }
212
currently_inside_ut(&self) -> &Option<UberTurn>213 pub fn currently_inside_ut(&self) -> &Option<UberTurn> {
214 &self.currently_inside_ut
215 }
about_to_start_ut(&self) -> Option<&UberTurn>216 pub fn about_to_start_ut(&self) -> Option<&UberTurn> {
217 if self.steps.len() < 2 || self.uber_turns.is_empty() {
218 return None;
219 }
220 if let PathStep::Turn(t) = self.steps[1] {
221 if self.uber_turns[0].path[0] == t {
222 return Some(&self.uber_turns[0]);
223 }
224 }
225 None
226 }
227
shift(&mut self, map: &Map) -> PathStep228 pub fn shift(&mut self, map: &Map) -> PathStep {
229 let step = self.steps.pop_front().unwrap();
230 self.crossed_so_far += step.as_traversable().length(map);
231
232 if let Some(ref ut) = self.currently_inside_ut {
233 if step == PathStep::Turn(*ut.path.last().unwrap()) {
234 self.currently_inside_ut = None;
235 }
236 } else if !self.steps.is_empty() && !self.uber_turns.is_empty() {
237 if self.steps[0] == PathStep::Turn(self.uber_turns[0].path[0]) {
238 self.currently_inside_ut = Some(self.uber_turns.pop_front().unwrap());
239 }
240 }
241
242 if self.steps.len() == 1 {
243 // TODO When handle_uber_turns experiment is turned off, this will crash
244 assert!(self.uber_turns.is_empty());
245 assert!(self.currently_inside_ut.is_none());
246 }
247
248 step
249 }
250
251 // TODO Maybe need to amend uber_turns?
add(&mut self, step: PathStep, map: &Map)252 pub fn add(&mut self, step: PathStep, map: &Map) {
253 self.total_length += step.as_traversable().length(map);
254 match step {
255 PathStep::Lane(_) | PathStep::ContraflowLane(_) => self.total_lanes += 1,
256 _ => {}
257 };
258 self.steps.push_back(step);
259 }
260
261 // TODO This is a brittle, tied to exactly what opportunistically_lanechange does.
approaching_uber_turn(&self) -> bool262 pub fn approaching_uber_turn(&self) -> bool {
263 if self.steps.len() < 5 || self.uber_turns.is_empty() {
264 return false;
265 }
266 if let PathStep::Turn(t) = self.steps[1] {
267 if self.uber_turns[0].path[0] == t {
268 return true;
269 }
270 }
271 if let PathStep::Turn(t) = self.steps[3] {
272 if self.uber_turns[0].path[0] == t {
273 return true;
274 }
275 }
276 false
277 }
278
279 // Trusting the caller to do this in valid ways.
modify_step(&mut self, idx: usize, step: PathStep, map: &Map)280 pub fn modify_step(&mut self, idx: usize, step: PathStep, map: &Map) {
281 assert!(self.currently_inside_ut.is_none());
282 assert!(idx != 0);
283 self.total_length -= self.steps[idx].as_traversable().length(map);
284 self.steps[idx] = step;
285 self.total_length += self.steps[idx].as_traversable().length(map);
286
287 if self.total_length < Distance::ZERO {
288 panic!(
289 "modify_step broke total_length, it's now {}",
290 self.total_length
291 );
292 }
293 }
294
current_step(&self) -> PathStep295 pub fn current_step(&self) -> PathStep {
296 self.steps[0]
297 }
298
next_step(&self) -> PathStep299 pub fn next_step(&self) -> PathStep {
300 self.steps[1]
301 }
302
last_step(&self) -> PathStep303 pub fn last_step(&self) -> PathStep {
304 self.steps[self.steps.len() - 1]
305 }
306
307 // dist_ahead is unlimited when None.
trace( &self, map: &Map, start_dist: Distance, dist_ahead: Option<Distance>, ) -> Option<PolyLine>308 pub fn trace(
309 &self,
310 map: &Map,
311 start_dist: Distance,
312 dist_ahead: Option<Distance>,
313 ) -> Option<PolyLine> {
314 let mut pts_so_far: Option<PolyLine> = None;
315 let mut dist_remaining = dist_ahead;
316
317 if self.steps.len() == 1 {
318 let dist = if start_dist < self.end_dist {
319 self.end_dist - start_dist
320 } else {
321 start_dist - self.end_dist
322 };
323 if let Some(d) = dist_remaining {
324 if dist < d {
325 dist_remaining = Some(dist);
326 }
327 } else {
328 dist_remaining = Some(dist);
329 }
330 }
331
332 // Special case the first step.
333 if let Ok((pts, dist)) = self.steps[0].slice(map, start_dist, dist_remaining) {
334 pts_so_far = Some(pts);
335 if dist_remaining.is_some() {
336 dist_remaining = Some(dist);
337 }
338 }
339
340 if self.steps.len() == 1 {
341 // It's possible there are paths on their last step that're effectively empty, because
342 // they're a 0-length turn, or something like a pedestrian crossing a front path and
343 // immediately getting on a bike.
344 return pts_so_far;
345 }
346
347 // Crunch through the intermediate steps, as long as we can.
348 for i in 1..self.steps.len() {
349 if let Some(d) = dist_remaining {
350 if d <= Distance::ZERO {
351 // We know there's at least some geometry if we made it here, so unwrap to
352 // verify that understanding.
353 return Some(pts_so_far.unwrap());
354 }
355 }
356 // If we made it to the last step, maybe use the end_dist.
357 if i == self.steps.len() - 1 {
358 let end_dist = match self.steps[i] {
359 PathStep::ContraflowLane(l) => {
360 map.get_l(l).lane_center_pts.reversed().length() - self.end_dist
361 }
362 _ => self.end_dist,
363 };
364 if let Some(d) = dist_remaining {
365 if end_dist < d {
366 dist_remaining = Some(end_dist);
367 }
368 } else {
369 dist_remaining = Some(end_dist);
370 }
371 }
372
373 let start_dist_this_step = match self.steps[i] {
374 // TODO Length of a PolyLine can slightly change when points are reversed! That
375 // seems bad.
376 PathStep::ContraflowLane(l) => map.get_l(l).lane_center_pts.reversed().length(),
377 _ => Distance::ZERO,
378 };
379 if let Ok((new_pts, dist)) =
380 self.steps[i].slice(map, start_dist_this_step, dist_remaining)
381 {
382 if pts_so_far.is_some() {
383 match pts_so_far.unwrap().extend(new_pts) {
384 Ok(new) => {
385 pts_so_far = Some(new);
386 }
387 Err(err) => {
388 println!("WARNING: Couldn't trace some path: {}", err);
389 return None;
390 }
391 }
392 } else {
393 pts_so_far = Some(new_pts);
394 }
395 if dist_remaining.is_some() {
396 dist_remaining = Some(dist);
397 }
398 }
399 }
400
401 Some(pts_so_far.unwrap())
402 }
403
get_steps(&self) -> &VecDeque<PathStep>404 pub fn get_steps(&self) -> &VecDeque<PathStep> {
405 &self.steps
406 }
407
408 // Not for walking paths
append(&mut self, other: Path, map: &Map)409 fn append(&mut self, other: Path, map: &Map) {
410 assert!(self.currently_inside_ut.is_none());
411 assert!(other.currently_inside_ut.is_none());
412 let turn = match (*self.steps.back().unwrap(), other.steps[0]) {
413 (PathStep::Lane(src), PathStep::Lane(dst)) => TurnID {
414 parent: map.get_l(src).dst_i,
415 src,
416 dst,
417 },
418 _ => unreachable!(),
419 };
420 self.steps.push_back(PathStep::Turn(turn));
421 self.total_length += map.get_t(turn).geom.length();
422 self.steps.extend(other.steps);
423 self.total_length += other.total_length;
424 self.total_lanes += other.total_lanes;
425 self.uber_turns.extend(other.uber_turns);
426 }
427 }
428
429 // Who's asking for a path?
430 // TODO This is an awful name.
431 #[derive(Debug, Serialize, Deserialize, PartialOrd, Ord, EnumSetType)]
432 pub enum PathConstraints {
433 Pedestrian,
434 Car,
435 Bike,
436 Bus,
437 Train,
438 }
439
440 impl PathConstraints {
all() -> Vec<PathConstraints>441 pub fn all() -> Vec<PathConstraints> {
442 vec![
443 PathConstraints::Pedestrian,
444 PathConstraints::Car,
445 PathConstraints::Bike,
446 PathConstraints::Bus,
447 PathConstraints::Train,
448 ]
449 }
450
451 // Not bijective, but this is the best guess of user intent
from_lt(lt: LaneType) -> PathConstraints452 pub fn from_lt(lt: LaneType) -> PathConstraints {
453 match lt {
454 LaneType::Sidewalk | LaneType::Shoulder => PathConstraints::Pedestrian,
455 LaneType::Driving => PathConstraints::Car,
456 LaneType::Biking => PathConstraints::Bike,
457 LaneType::Bus => PathConstraints::Bus,
458 LaneType::LightRail => PathConstraints::Train,
459 _ => panic!("PathConstraints::from_lt({:?}) doesn't make sense", lt),
460 }
461 }
462
463 // TODO Handle private zones here?
can_use(self, l: &Lane, map: &Map) -> bool464 pub fn can_use(self, l: &Lane, map: &Map) -> bool {
465 match self {
466 PathConstraints::Pedestrian => l.is_walkable(),
467 PathConstraints::Car => l.is_driving(),
468 PathConstraints::Bike => {
469 if l.is_biking() {
470 true
471 } else if l.is_driving() || (l.is_bus() && map.config.bikes_can_use_bus_lanes) {
472 let road = map.get_r(l.parent);
473 !road.osm_tags.is("bicycle", "no")
474 && !road
475 .osm_tags
476 .is_any(osm::HIGHWAY, vec!["motorway", "motorway_link"])
477 } else {
478 false
479 }
480 }
481 PathConstraints::Bus => l.is_driving() || l.is_bus(),
482 PathConstraints::Train => l.is_light_rail(),
483 }
484 }
485
486 // Strict for bikes. If there are bike lanes, not allowed to use other lanes.
filter_lanes(self, mut choices: Vec<LaneID>, map: &Map) -> Vec<LaneID>487 pub(crate) fn filter_lanes(self, mut choices: Vec<LaneID>, map: &Map) -> Vec<LaneID> {
488 choices.retain(|l| self.can_use(map.get_l(*l), map));
489 if self == PathConstraints::Bike {
490 let just_bike_lanes: Vec<LaneID> = choices
491 .iter()
492 .copied()
493 .filter(|l| map.get_l(*l).is_biking())
494 .collect();
495 if !just_bike_lanes.is_empty() {
496 return just_bike_lanes;
497 }
498 }
499 choices
500 }
501 }
502
503 #[derive(Debug, PartialEq, Eq, Clone, Serialize, Deserialize)]
504 pub struct PathRequest {
505 pub start: Position,
506 pub end: Position,
507 pub constraints: PathConstraints,
508 }
509
510 impl fmt::Display for PathRequest {
fmt(&self, f: &mut fmt::Formatter) -> fmt::Result511 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
512 write!(
513 f,
514 "PathRequest({} along {}... to {} along {} for {:?})",
515 self.start.dist_along(),
516 self.start.lane(),
517 self.end.dist_along(),
518 self.end.lane(),
519 self.constraints,
520 )
521 }
522 }
523
validate_continuity(map: &Map, steps: &Vec<PathStep>)524 fn validate_continuity(map: &Map, steps: &Vec<PathStep>) {
525 if steps.is_empty() {
526 panic!("Empty path");
527 }
528 for pair in steps.windows(2) {
529 let from = match pair[0] {
530 PathStep::Lane(id) => map.get_l(id).last_pt(),
531 PathStep::ContraflowLane(id) => map.get_l(id).first_pt(),
532 PathStep::Turn(id) => map.get_t(id).geom.last_pt(),
533 };
534 let to = match pair[1] {
535 PathStep::Lane(id) => map.get_l(id).first_pt(),
536 PathStep::ContraflowLane(id) => map.get_l(id).last_pt(),
537 PathStep::Turn(id) => map.get_t(id).geom.first_pt(),
538 };
539 let len = from.dist_to(to);
540 if len > EPSILON_DIST {
541 println!("All steps in invalid path:");
542 for s in steps {
543 match s {
544 PathStep::Lane(l) => println!(
545 " {:?} from {} to {}",
546 s,
547 map.get_l(*l).src_i,
548 map.get_l(*l).dst_i
549 ),
550 PathStep::ContraflowLane(l) => println!(
551 " {:?} from {} to {}",
552 s,
553 map.get_l(*l).dst_i,
554 map.get_l(*l).src_i
555 ),
556 PathStep::Turn(_) => println!(" {:?}", s),
557 }
558 }
559 panic!(
560 "pathfind() returned path that warps {} from {:?} to {:?}",
561 len, pair[0], pair[1]
562 );
563 }
564 }
565 }
566
validate_restrictions(map: &Map, steps: &Vec<PathStep>)567 fn validate_restrictions(map: &Map, steps: &Vec<PathStep>) {
568 for triple in steps.windows(5) {
569 if let (PathStep::Lane(l1), PathStep::Lane(l2), PathStep::Lane(l3)) =
570 (triple[0], triple[2], triple[4])
571 {
572 let from = map.get_parent(l1);
573 let via = map.get_l(l2).parent;
574 let to = map.get_l(l3).parent;
575
576 for (dont_via, dont_to) in &from.complicated_turn_restrictions {
577 if via == *dont_via && to == *dont_to {
578 panic!(
579 "Some path does illegal uber-turn: {} -> {} -> {}",
580 l1, l2, l3
581 );
582 }
583 }
584 }
585 }
586 }
587
588 // Most of the time, prefer using the faster contraction hierarchies. But sometimes, callers can
589 // explicitly opt into a slower (but preparation-free) pathfinder that just uses Dijkstra's
590 // maneuever.
591 #[derive(Serialize, Deserialize)]
592 pub enum Pathfinder {
593 Dijkstra,
594 CH(ContractionHierarchyPathfinder),
595 }
596
597 impl Pathfinder {
pathfind(&self, req: PathRequest, map: &Map) -> Option<Path>598 pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option<Path> {
599 match self {
600 Pathfinder::Dijkstra => dijkstra::pathfind(req, map),
601 Pathfinder::CH(ref p) => p.pathfind(req, map),
602 }
603 }
604
should_use_transit( &self, map: &Map, start: Position, end: Position, ) -> Option<(BusStopID, Option<BusStopID>, BusRouteID)>605 pub fn should_use_transit(
606 &self,
607 map: &Map,
608 start: Position,
609 end: Position,
610 ) -> Option<(BusStopID, Option<BusStopID>, BusRouteID)> {
611 match self {
612 // TODO Implement this
613 Pathfinder::Dijkstra => None,
614 Pathfinder::CH(ref p) => p.should_use_transit(map, start, end),
615 }
616 }
617
apply_edits(&mut self, map: &Map, timer: &mut Timer)618 pub fn apply_edits(&mut self, map: &Map, timer: &mut Timer) {
619 match self {
620 Pathfinder::Dijkstra => {}
621 Pathfinder::CH(ref mut p) => p.apply_edits(map, timer),
622 }
623 }
624 }
625