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