1 use crate::pathfind::driving::VehiclePathfinder;
2 use crate::pathfind::walking::{
3     one_step_walking_path, walking_path_to_steps, SidewalkPathfinder, WalkingNode,
4 };
5 use crate::{
6     BusRouteID, BusStopID, Intersection, Map, Path, PathConstraints, PathRequest, Position, TurnID,
7     Zone,
8 };
9 use abstutil::Timer;
10 use serde::{Deserialize, Serialize};
11 
12 #[derive(Serialize, Deserialize)]
13 pub struct ContractionHierarchyPathfinder {
14     car_graph: VehiclePathfinder,
15     bike_graph: VehiclePathfinder,
16     bus_graph: VehiclePathfinder,
17     train_graph: VehiclePathfinder,
18     walking_graph: SidewalkPathfinder,
19     walking_with_transit_graph: SidewalkPathfinder,
20 }
21 
22 impl ContractionHierarchyPathfinder {
new(map: &Map, timer: &mut Timer) -> ContractionHierarchyPathfinder23     pub fn new(map: &Map, timer: &mut Timer) -> ContractionHierarchyPathfinder {
24         timer.start("prepare pathfinding for cars");
25         let car_graph = VehiclePathfinder::new(map, PathConstraints::Car, None);
26         timer.stop("prepare pathfinding for cars");
27 
28         // The edge weights for bikes are so different from the driving graph that reusing the node
29         // ordering actually hurts!
30         timer.start("prepare pathfinding for bikes");
31         let bike_graph = VehiclePathfinder::new(map, PathConstraints::Bike, None);
32         timer.stop("prepare pathfinding for bikes");
33 
34         timer.start("prepare pathfinding for buses");
35         let bus_graph = VehiclePathfinder::new(map, PathConstraints::Bus, Some(&car_graph));
36         timer.stop("prepare pathfinding for buses");
37 
38         timer.start("prepare pathfinding for trains");
39         let train_graph = VehiclePathfinder::new(map, PathConstraints::Train, None);
40         timer.stop("prepare pathfinding for trains");
41 
42         timer.start("prepare pathfinding for pedestrians");
43         let walking_graph = SidewalkPathfinder::new(map, false, &bus_graph, &train_graph);
44         timer.stop("prepare pathfinding for pedestrians");
45 
46         timer.start("prepare pathfinding for pedestrians using transit");
47         let walking_with_transit_graph =
48             SidewalkPathfinder::new(map, true, &bus_graph, &train_graph);
49         timer.stop("prepare pathfinding for pedestrians using transit");
50 
51         ContractionHierarchyPathfinder {
52             car_graph,
53             bike_graph,
54             bus_graph,
55             train_graph,
56             walking_graph,
57             walking_with_transit_graph,
58         }
59     }
60 
pathfind(&self, req: PathRequest, map: &Map) -> Option<Path>61     pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option<Path> {
62         if req.start.lane() == req.end.lane() && req.constraints == PathConstraints::Pedestrian {
63             return Some(one_step_walking_path(&req, map));
64         }
65 
66         // If we start or end in a private zone, have to stitch together a smaller path with a path
67         // through the main map.
68         let start_r = map.get_parent(req.start.lane());
69         let end_r = map.get_parent(req.end.lane());
70 
71         match (start_r.get_zone(map), end_r.get_zone(map)) {
72             (Some(z1), Some(z2)) => {
73                 if z1 == z2 {
74                     if !z1
75                         .restrictions
76                         .allow_through_traffic
77                         .contains(req.constraints)
78                     {
79                         if req.constraints == PathConstraints::Pedestrian {
80                             let steps =
81                                 walking_path_to_steps(z1.pathfind_walking(req.clone(), map)?, map);
82                             return Some(Path::new(map, steps, req.end.dist_along(), Vec::new()));
83                         }
84                         return z1.pathfind(req, map);
85                     }
86                 } else {
87                     // TODO Handle paths going between two different zones
88                     return None;
89                 }
90             }
91             (Some(zone), None) => {
92                 if !zone
93                     .restrictions
94                     .allow_through_traffic
95                     .contains(req.constraints)
96                 {
97                     let mut borders: Vec<&Intersection> =
98                         zone.borders.iter().map(|i| map.get_i(*i)).collect();
99                     // TODO Use the CH to pick the lowest overall cost?
100                     let pt = req.end.pt(map);
101                     borders.sort_by_key(|i| pt.dist_to(i.polygon.center()));
102 
103                     for i in borders {
104                         if let Some(result) = self.pathfind_from_zone(i, req.clone(), zone, map) {
105                             return Some(result);
106                         }
107                     }
108                     return None;
109                 }
110             }
111             (None, Some(zone)) => {
112                 if !zone
113                     .restrictions
114                     .allow_through_traffic
115                     .contains(req.constraints)
116                 {
117                     let mut borders: Vec<&Intersection> =
118                         zone.borders.iter().map(|i| map.get_i(*i)).collect();
119                     // TODO Use the CH to pick the lowest overall cost?
120                     let pt = req.start.pt(map);
121                     borders.sort_by_key(|i| pt.dist_to(i.polygon.center()));
122 
123                     for i in borders {
124                         if let Some(result) = self.pathfind_to_zone(i, req.clone(), zone, map) {
125                             return Some(result);
126                         }
127                     }
128                     return None;
129                 }
130             }
131             (None, None) => {}
132         }
133         match req.constraints {
134             PathConstraints::Pedestrian => {
135                 let steps = walking_path_to_steps(self.walking_graph.pathfind(&req, map)?, map);
136                 Some(Path::new(map, steps, req.end.dist_along(), Vec::new()))
137             }
138             PathConstraints::Car => self.car_graph.pathfind(&req, map).map(|(p, _)| p),
139             PathConstraints::Bike => self.bike_graph.pathfind(&req, map).map(|(p, _)| p),
140             PathConstraints::Bus => self.bus_graph.pathfind(&req, map).map(|(p, _)| p),
141             PathConstraints::Train => self.train_graph.pathfind(&req, map).map(|(p, _)| p),
142         }
143     }
144 
145     // TODO Alright, reconsider refactoring pieces of this again. :)
pathfind_from_zone( &self, i: &Intersection, mut req: PathRequest, zone: &Zone, map: &Map, ) -> Option<Path>146     fn pathfind_from_zone(
147         &self,
148         i: &Intersection,
149         mut req: PathRequest,
150         zone: &Zone,
151         map: &Map,
152     ) -> Option<Path> {
153         // Because sidewalks aren't all immediately linked, insist on a (src, dst) combo that
154         // are actually connected by a turn.
155         let src_choices = i
156             .get_incoming_lanes(map, req.constraints)
157             .filter(|l| zone.members.contains(&map.get_l(*l).parent))
158             .collect::<Vec<_>>();
159         let dst_choices = i
160             .get_outgoing_lanes(map, req.constraints)
161             .into_iter()
162             .filter(|l| !zone.members.contains(&map.get_l(*l).parent))
163             .collect::<Vec<_>>();
164         let (src, dst) = {
165             let mut result = None;
166             'OUTER: for l1 in src_choices {
167                 for l2 in &dst_choices {
168                     if l1 != *l2
169                         && map
170                             .maybe_get_t(TurnID {
171                                 parent: i.id,
172                                 src: l1,
173                                 dst: *l2,
174                             })
175                             .is_some()
176                     {
177                         result = Some((l1, *l2));
178                         break 'OUTER;
179                     }
180                 }
181             }
182             result?
183         };
184 
185         let interior_req = PathRequest {
186             start: req.start,
187             end: if map.get_l(src).dst_i == i.id {
188                 Position::end(src, map)
189             } else {
190                 Position::start(src)
191             },
192             constraints: req.constraints,
193         };
194         req.start = if map.get_l(dst).src_i == i.id {
195             Position::start(dst)
196         } else {
197             Position::end(dst, map)
198         };
199 
200         if let PathConstraints::Pedestrian = req.constraints {
201             let mut interior_path = zone.pathfind_walking(interior_req, map)?;
202             let main_path = if req.start.lane() == req.end.lane() {
203                 let mut one_step = vec![
204                     WalkingNode::closest(req.start, map),
205                     WalkingNode::closest(req.end, map),
206                 ];
207                 one_step.dedup();
208                 one_step
209             } else {
210                 self.walking_graph.pathfind(&req, map)?
211             };
212             interior_path.extend(main_path);
213             let steps = walking_path_to_steps(interior_path, map);
214             return Some(Path::new(map, steps, req.end.dist_along(), Vec::new()));
215         }
216 
217         let mut interior_path = zone.pathfind(interior_req, map)?;
218         let main_path = match req.constraints {
219             PathConstraints::Pedestrian => unreachable!(),
220             PathConstraints::Car => self.car_graph.pathfind(&req, map).map(|(p, _)| p),
221             PathConstraints::Bike => self.bike_graph.pathfind(&req, map).map(|(p, _)| p),
222             PathConstraints::Bus => self.bus_graph.pathfind(&req, map).map(|(p, _)| p),
223             PathConstraints::Train => self.train_graph.pathfind(&req, map).map(|(p, _)| p),
224         }?;
225         interior_path.append(main_path, map);
226         Some(interior_path)
227     }
228 
pathfind_to_zone( &self, i: &Intersection, mut req: PathRequest, zone: &Zone, map: &Map, ) -> Option<Path>229     fn pathfind_to_zone(
230         &self,
231         i: &Intersection,
232         mut req: PathRequest,
233         zone: &Zone,
234         map: &Map,
235     ) -> Option<Path> {
236         // Because sidewalks aren't all immediately linked, insist on a (src, dst) combo that
237         // are actually connected by a turn.
238         let src_choices = i
239             .get_incoming_lanes(map, req.constraints)
240             .filter(|l| !zone.members.contains(&map.get_l(*l).parent))
241             .collect::<Vec<_>>();
242         let dst_choices = i
243             .get_outgoing_lanes(map, req.constraints)
244             .into_iter()
245             .filter(|l| zone.members.contains(&map.get_l(*l).parent))
246             .collect::<Vec<_>>();
247         let (src, dst) = {
248             let mut result = None;
249             'OUTER: for l1 in src_choices {
250                 for l2 in &dst_choices {
251                     if l1 != *l2
252                         && map
253                             .maybe_get_t(TurnID {
254                                 parent: i.id,
255                                 src: l1,
256                                 dst: *l2,
257                             })
258                             .is_some()
259                     {
260                         result = Some((l1, *l2));
261                         break 'OUTER;
262                     }
263                 }
264             }
265             result?
266         };
267 
268         let interior_req = PathRequest {
269             start: if map.get_l(dst).src_i == i.id {
270                 Position::start(dst)
271             } else {
272                 Position::end(dst, map)
273             },
274             end: req.end,
275             constraints: req.constraints,
276         };
277         let orig_end_dist = req.end.dist_along();
278         req.end = if map.get_l(src).dst_i == i.id {
279             Position::end(src, map)
280         } else {
281             Position::start(src)
282         };
283 
284         if let PathConstraints::Pedestrian = req.constraints {
285             let interior_path = zone.pathfind_walking(interior_req, map)?;
286             let mut main_path = if req.start.lane() == req.end.lane() {
287                 let mut one_step = vec![
288                     WalkingNode::closest(req.start, map),
289                     WalkingNode::closest(req.end, map),
290                 ];
291                 one_step.dedup();
292                 one_step
293             } else {
294                 self.walking_graph.pathfind(&req, map)?
295             };
296 
297             main_path.extend(interior_path);
298             let steps = walking_path_to_steps(main_path, map);
299             return Some(Path::new(map, steps, orig_end_dist, Vec::new()));
300         }
301 
302         let interior_path = zone.pathfind(interior_req, map)?;
303         let mut main_path = match req.constraints {
304             PathConstraints::Pedestrian => unreachable!(),
305             PathConstraints::Car => self.car_graph.pathfind(&req, map).map(|(p, _)| p),
306             PathConstraints::Bike => self.bike_graph.pathfind(&req, map).map(|(p, _)| p),
307             PathConstraints::Bus => self.bus_graph.pathfind(&req, map).map(|(p, _)| p),
308             PathConstraints::Train => self.train_graph.pathfind(&req, map).map(|(p, _)| p),
309         }?;
310         main_path.append(interior_path, map);
311         main_path.end_dist = orig_end_dist;
312         Some(main_path)
313     }
314 
should_use_transit( &self, map: &Map, start: Position, end: Position, ) -> Option<(BusStopID, Option<BusStopID>, BusRouteID)>315     pub fn should_use_transit(
316         &self,
317         map: &Map,
318         start: Position,
319         end: Position,
320     ) -> Option<(BusStopID, Option<BusStopID>, BusRouteID)> {
321         self.walking_with_transit_graph
322             .should_use_transit(map, start, end)
323     }
324 
apply_edits(&mut self, map: &Map, timer: &mut Timer)325     pub fn apply_edits(&mut self, map: &Map, timer: &mut Timer) {
326         timer.start("apply edits to car pathfinding");
327         self.car_graph.apply_edits(map);
328         timer.stop("apply edits to car pathfinding");
329 
330         timer.start("apply edits to bike pathfinding");
331         self.bike_graph.apply_edits(map);
332         timer.stop("apply edits to bike pathfinding");
333 
334         timer.start("apply edits to bus pathfinding");
335         self.bus_graph.apply_edits(map);
336         timer.stop("apply edits to bus pathfinding");
337 
338         // Can't edit anything related to trains
339 
340         timer.start("apply edits to pedestrian pathfinding");
341         self.walking_graph
342             .apply_edits(map, &self.bus_graph, &self.train_graph);
343         timer.stop("apply edits to pedestrian pathfinding");
344 
345         timer.start("apply edits to pedestrian using transit pathfinding");
346         self.walking_with_transit_graph
347             .apply_edits(map, &self.bus_graph, &self.train_graph);
348         timer.stop("apply edits to pedestrian using transit pathfinding");
349     }
350 }
351