diff --git a/map_model/src/objects/zone.rs b/map_model/src/objects/zone.rs index 4abfa7b57e..f8a80ffffc 100644 --- a/map_model/src/objects/zone.rs +++ b/map_model/src/objects/zone.rs @@ -8,13 +8,9 @@ use std::collections::BTreeSet; use enumset::EnumSet; -use petgraph::graphmap::DiGraphMap; use serde::{Deserialize, Serialize}; -use crate::pathfind::{driving_cost, walking_cost, WalkingNode}; -use crate::{ - IntersectionID, LaneID, Map, Path, PathConstraints, PathRequest, PathStep, RoadID, TurnID, -}; +use crate::{IntersectionID, Map, PathConstraints, RoadID}; #[derive(Serialize, Deserialize, Debug, PartialEq, Clone)] pub struct AccessRestrictions { @@ -63,94 +59,6 @@ impl Zone { zones } - - /// Run slower Dijkstra's within the interior of a private zone. Don't go outside the borders. - pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option { - assert_ne!(req.constraints, PathConstraints::Pedestrian); - - let mut graph: DiGraphMap = DiGraphMap::new(); - for r in &self.members { - for l in map.get_r(*r).all_lanes() { - if req.constraints.can_use(map.get_l(l), map) { - for turn in map.get_turns_for(l, req.constraints) { - if !self.borders.contains(&turn.id.parent) { - graph.add_edge(turn.id.src, turn.id.dst, turn.id); - } - } - } - } - } - - let (_, path) = petgraph::algo::astar( - &graph, - req.start.lane(), - |l| l == req.end.lane(), - |(_, _, turn)| { - driving_cost( - map.get_l(turn.src), - map.get_t(*turn), - req.constraints, - map.routing_params(), - map, - ) - }, - |_| 0.0, - )?; - let mut steps = Vec::new(); - for pair in path.windows(2) { - steps.push(PathStep::Lane(pair[0])); - // We don't need to look for this turn in the map; we know it exists. - steps.push(PathStep::Turn(TurnID { - parent: map.get_l(pair[0]).dst_i, - src: pair[0], - dst: pair[1], - })); - } - steps.push(PathStep::Lane(req.end.lane())); - assert_eq!(steps[0], PathStep::Lane(req.start.lane())); - Some(Path::new(map, steps, req, Vec::new())) - } - - // TODO Not happy this works so differently - pub fn pathfind_walking(&self, req: PathRequest, map: &Map) -> Option> { - let mut graph: DiGraphMap = DiGraphMap::new(); - for r in &self.members { - for l in map.get_r(*r).all_lanes() { - let l = map.get_l(l); - if l.is_walkable() { - let cost = walking_cost(l.length()); - let n1 = WalkingNode::SidewalkEndpoint(l.id, true); - let n2 = WalkingNode::SidewalkEndpoint(l.id, false); - graph.add_edge(n1, n2, cost); - graph.add_edge(n2, n1, cost); - - for turn in map.get_turns_for(l.id, PathConstraints::Pedestrian) { - if self.members.contains(&map.get_l(turn.id.dst).parent) { - graph.add_edge( - WalkingNode::SidewalkEndpoint(l.id, l.dst_i == turn.id.parent), - WalkingNode::SidewalkEndpoint( - turn.id.dst, - map.get_l(turn.id.dst).dst_i == turn.id.parent, - ), - walking_cost(turn.geom.length()), - ); - } - } - } - } - } - - let closest_start = WalkingNode::closest(req.start, map); - let closest_end = WalkingNode::closest(req.end, map); - let (_, path) = petgraph::algo::astar( - &graph, - closest_start, - |end| end == closest_end, - |(_, _, cost)| *cost, - |_| 0, - )?; - Some(path) - } } fn floodfill(map: &Map, start: RoadID) -> Zone { diff --git a/map_model/src/pathfind/driving.rs b/map_model/src/pathfind/driving.rs index 2fe128cbd3..9a1ed80362 100644 --- a/map_model/src/pathfind/driving.rs +++ b/map_model/src/pathfind/driving.rs @@ -167,13 +167,7 @@ fn make_input_graph( for l in map.all_lanes() { let from = nodes.get(Node::Lane(l.id)); let mut any = false; - if constraints.can_use(l, map) - && map - .get_r(l.parent) - .access_restrictions - .allow_through_traffic - .contains(constraints) - { + if constraints.can_use(l, map) { let indices = uber_turn_entrances.get(l.id); if indices.is_empty() { for turn in map.get_turns_for(l.id, constraints) { diff --git a/map_model/src/pathfind/mod.rs b/map_model/src/pathfind/mod.rs index c3328e4f7d..4a5d46358f 100644 --- a/map_model/src/pathfind/mod.rs +++ b/map_model/src/pathfind/mod.rs @@ -410,26 +410,6 @@ impl Path { &self.steps } - // Not for walking paths - fn append(&mut self, other: Path, map: &Map) { - assert!(self.currently_inside_ut.is_none()); - assert!(other.currently_inside_ut.is_none()); - let turn = match (*self.steps.back().unwrap(), other.steps[0]) { - (PathStep::Lane(src), PathStep::Lane(dst)) => TurnID { - parent: map.get_l(src).dst_i, - src, - dst, - }, - _ => unreachable!(), - }; - self.steps.push_back(PathStep::Turn(turn)); - // TODO Need to correct for the uncrossed start/end distance where we're gluing together - self.total_length += map.get_t(turn).geom.length(); - self.steps.extend(other.steps); - self.total_length += other.total_length; - self.uber_turns.extend(other.uber_turns); - } - /// Estimate how long following the path will take in the best case, assuming no traffic or /// delay at intersections. To determine the speed along each step, the agent following their /// path and their optional max_speed must be specified. @@ -491,7 +471,6 @@ impl PathConstraints { } } - // TODO Handle private zones here? pub fn can_use(self, l: &Lane, map: &Map) -> bool { match self { PathConstraints::Pedestrian => l.is_walkable(), diff --git a/map_model/src/pathfind/pathfinder.rs b/map_model/src/pathfind/pathfinder.rs index 29caf3247f..1ca6111301 100644 --- a/map_model/src/pathfind/pathfinder.rs +++ b/map_model/src/pathfind/pathfinder.rs @@ -5,11 +5,10 @@ use serde::{Deserialize, Serialize}; use abstutil::Timer; use crate::pathfind::ch::ContractionHierarchyPathfinder; +use crate::pathfind::dijkstra; use crate::pathfind::walking::{one_step_walking_path, walking_path_to_steps}; -use crate::pathfind::{dijkstra, WalkingNode}; use crate::{ - BusRouteID, BusStopID, Intersection, LaneID, Map, Path, PathConstraints, PathRequest, Position, - RoutingParams, TurnID, Zone, + BusRouteID, BusStopID, LaneID, Map, Path, PathConstraints, PathRequest, Position, RoutingParams, }; /// Most of the time, prefer using the faster contraction hierarchies. But sometimes, callers can @@ -22,14 +21,13 @@ pub enum Pathfinder { } impl Pathfinder { - /// Finds a path from a start to an end for a certain type of agent. Handles requests that - /// start or end inside access-restricted zones. + /// Finds a path from a start to an end for a certain type of agent. pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option { self.pathfind_with_params(req, map.routing_params(), map) } - /// Finds a path from a start to an end for a certain type of agent. Handles requests that - /// start or end inside access-restricted zones. May use custom routing parameters. + /// Finds a path from a start to an end for a certain type of agent. May use custom routing + /// parameters. pub fn pathfind_with_params( &self, req: PathRequest, @@ -40,82 +38,30 @@ impl Pathfinder { return Some(one_step_walking_path(&req, map)); } - // If we start or end in a private zone, have to stitch together a smaller path with a path - // through the main map. - let start_r = map.get_parent(req.start.lane()); - let end_r = map.get_parent(req.end.lane()); - - match (start_r.get_zone(map), end_r.get_zone(map)) { - (Some(z1), Some(z2)) => { - if z1 == z2 { - if !z1 - .restrictions - .allow_through_traffic - .contains(req.constraints) - { - if req.constraints == PathConstraints::Pedestrian { - let steps = - walking_path_to_steps(z1.pathfind_walking(req.clone(), map)?, map); - return Some(Path::new(map, steps, req, Vec::new())); - } - return z1.pathfind(req, map); - } - } else { - // TODO Handle paths going between two different zones - return None; - } - } - (Some(zone), None) => { - if !zone - .restrictions - .allow_through_traffic - .contains(req.constraints) - { - // Calculate the entire path using every possible border, then take the one - // with the least total distance. - // TODO This is slow and doesn't account for the mode-specific cost. - let mut paths = Vec::new(); - for i in &zone.borders { - if let Some(result) = - self.pathfind_from_zone(map.get_i(*i), req.clone(), zone, map) - { - paths.push(result); - } - } - return paths.into_iter().min_by_key(|p| p.total_length()); - } - } - (None, Some(zone)) => { - if !zone - .restrictions - .allow_through_traffic - .contains(req.constraints) - { - // Calculate the entire path using every possible border, then take the one - // with the least total distance. - // TODO This is slow and doesn't account for the mode-specific cost. - let mut paths = Vec::new(); - for i in &zone.borders { - if let Some(result) = - self.pathfind_to_zone(map.get_i(*i), req.clone(), zone, map) - { - paths.push(result); - } - } - return paths.into_iter().min_by_key(|p| p.total_length()); - } - } - (None, None) => {} - } - if req.constraints == PathConstraints::Pedestrian { if req.start.lane() == req.end.lane() { return Some(one_step_walking_path(&req, map)); } - let steps = walking_path_to_steps(self.simple_walking_path(&req, map)?, map); + let nodes = match self { + Pathfinder::Dijkstra => dijkstra::simple_walking_path(&req, map)?, + Pathfinder::CH(ref p) => p.simple_walking_path(&req, map)?, + }; + let steps = walking_path_to_steps(nodes, map); return Some(Path::new(map, steps, req, Vec::new())); } - self.simple_pathfind(&req, params, map) + + if params != map.routing_params() { + // If the params differ from the ones baked into the map, the CHs won't match. This + // should only be happening from the debug UI; be very obnoxious if we start calling it + // from the simulation or something else. + warn!("Pathfinding slowly for {} with custom params", req); + return dijkstra::simple_pathfind(&req, params, map); + } + + match self { + Pathfinder::Dijkstra => dijkstra::simple_pathfind(&req, params, map), + Pathfinder::CH(ref p) => p.simple_pathfind(&req, map), + } } pub fn pathfind_avoiding_lanes( @@ -147,193 +93,4 @@ impl Pathfinder { Pathfinder::CH(ref mut p) => p.apply_edits(map, timer), } } - - // Doesn't handle zones or pedestrians - fn simple_pathfind( - &self, - req: &PathRequest, - params: &RoutingParams, - map: &Map, - ) -> Option { - if params != map.routing_params() { - // If the params differ from the ones baked into the map, the CHs won't match. This - // should only be happening from the debug UI; be very obnoxious if we start calling it - // from the simulation or something else. - warn!("Pathfinding slowly for {} with custom params", req); - return dijkstra::simple_pathfind(req, params, map); - } - - match self { - Pathfinder::Dijkstra => dijkstra::simple_pathfind(req, params, map), - Pathfinder::CH(ref p) => p.simple_pathfind(req, map), - } - } - - fn simple_walking_path(&self, req: &PathRequest, map: &Map) -> Option> { - match self { - Pathfinder::Dijkstra => dijkstra::simple_walking_path(req, map), - Pathfinder::CH(ref p) => p.simple_walking_path(req, map), - } - } - - fn pathfind_from_zone( - &self, - i: &Intersection, - mut req: PathRequest, - zone: &Zone, - map: &Map, - ) -> Option { - // Because sidewalks aren't all immediately linked, insist on a (src, dst) combo that - // are actually connected by a turn. - let src_choices = i - .get_incoming_lanes(map, req.constraints) - .into_iter() - .filter(|l| zone.members.contains(&map.get_l(*l).parent)) - .collect::>(); - let dst_choices = i - .get_outgoing_lanes(map, req.constraints) - .into_iter() - .filter(|l| !zone.members.contains(&map.get_l(*l).parent)) - .collect::>(); - let (src, dst) = { - let mut result = None; - 'OUTER: for l1 in src_choices { - for l2 in &dst_choices { - if l1 != *l2 - && map - .maybe_get_t(TurnID { - parent: i.id, - src: l1, - dst: *l2, - }) - .is_some() - { - result = Some((l1, *l2)); - break 'OUTER; - } - } - } - result? - }; - - let interior_req = PathRequest { - start: req.start, - end: if map.get_l(src).dst_i == i.id { - Position::end(src, map) - } else { - Position::start(src) - }, - constraints: req.constraints, - }; - let orig_req = req.clone(); - req.start = if map.get_l(dst).src_i == i.id { - Position::start(dst) - } else { - Position::end(dst, map) - }; - - if let PathConstraints::Pedestrian = req.constraints { - let mut interior_path = zone.pathfind_walking(interior_req, map)?; - let main_path = if req.start.lane() == req.end.lane() { - let mut one_step = vec![ - WalkingNode::closest(req.start, map), - WalkingNode::closest(req.end, map), - ]; - one_step.dedup(); - one_step - } else { - self.simple_walking_path(&req, map)? - }; - interior_path.extend(main_path); - let steps = walking_path_to_steps(interior_path, map); - return Some(Path::new(map, steps, orig_req, Vec::new())); - } - - let mut interior_path = zone.pathfind(interior_req, map)?; - let main_path = self.simple_pathfind(&req, map.routing_params(), map)?; - interior_path.append(main_path, map); - interior_path.orig_req = orig_req; - Some(interior_path) - } - - fn pathfind_to_zone( - &self, - i: &Intersection, - mut req: PathRequest, - zone: &Zone, - map: &Map, - ) -> Option { - // Because sidewalks aren't all immediately linked, insist on a (src, dst) combo that - // are actually connected by a turn. - let src_choices = i - .get_incoming_lanes(map, req.constraints) - .into_iter() - .filter(|l| !zone.members.contains(&map.get_l(*l).parent)) - .collect::>(); - let dst_choices = i - .get_outgoing_lanes(map, req.constraints) - .into_iter() - .filter(|l| zone.members.contains(&map.get_l(*l).parent)) - .collect::>(); - let (src, dst) = { - let mut result = None; - 'OUTER: for l1 in src_choices { - for l2 in &dst_choices { - if l1 != *l2 - && map - .maybe_get_t(TurnID { - parent: i.id, - src: l1, - dst: *l2, - }) - .is_some() - { - result = Some((l1, *l2)); - break 'OUTER; - } - } - } - result? - }; - - let interior_req = PathRequest { - start: if map.get_l(dst).src_i == i.id { - Position::start(dst) - } else { - Position::end(dst, map) - }, - end: req.end, - constraints: req.constraints, - }; - let orig_req = req.clone(); - req.end = if map.get_l(src).dst_i == i.id { - Position::end(src, map) - } else { - Position::start(src) - }; - - if let PathConstraints::Pedestrian = req.constraints { - let interior_path = zone.pathfind_walking(interior_req, map)?; - let mut main_path = if req.start.lane() == req.end.lane() { - let mut one_step = vec![ - WalkingNode::closest(req.start, map), - WalkingNode::closest(req.end, map), - ]; - one_step.dedup(); - one_step - } else { - self.simple_walking_path(&req, map)? - }; - - main_path.extend(interior_path); - let steps = walking_path_to_steps(main_path, map); - return Some(Path::new(map, steps, orig_req, Vec::new())); - } - - let interior_path = zone.pathfind(interior_req, map)?; - let mut main_path = self.simple_pathfind(&req, map.routing_params(), map)?; - main_path.append(interior_path, map); - main_path.orig_req = orig_req; - Some(main_path) - } } diff --git a/map_model/src/pathfind/walking.rs b/map_model/src/pathfind/walking.rs index 3f7cfa7d69..476b993fbc 100644 --- a/map_model/src/pathfind/walking.rs +++ b/map_model/src/pathfind/walking.rs @@ -232,13 +232,7 @@ fn make_input_graph( let mut input_graph = InputGraph::new(); for l in map.all_lanes() { - if l.is_walkable() - && map - .get_r(l.parent) - .access_restrictions - .allow_through_traffic - .contains(PathConstraints::Pedestrian) - { + if l.is_walkable() { let mut cost = walking_cost(l.length()); // TODO Tune this penalty, along with many others. if l.is_shoulder() {