try something simpler for two-step pathfinding. maybe some code duplication, but way easier to reason about. i think this is now correct; just need equivalent for walking trips

map_model/src/pathfind/mod.rs

@@ -8,7 +8,8 @@ pub use self::driving::cost;
 use self::driving::VehiclePathfinder;
 use self::walking::SidewalkPathfinder;
 use crate::{
-    osm, BusRouteID, BusStopID, Lane, LaneID, LaneType, Map, Position, Traversable, TurnID,
+    osm, BusRouteID, BusStopID, Intersection, Lane, LaneID, LaneType, Map, Position, Traversable,
+    TurnID, Zone,
 };
 use abstutil::Timer;
 use geom::{Distance, PolyLine, EPSILON_DIST};
@@ -317,6 +318,44 @@ impl Path {
     pub fn get_steps(&self) -> &VecDeque<PathStep> {
         &self.steps
     }
+
+    fn prepend(&mut self, other: Path, map: &Map) {
+        match (*other.steps.back().unwrap(), self.steps[0]) {
+            (PathStep::Lane(src), PathStep::Lane(dst)) => {
+                let turn = TurnID {
+                    parent: map.get_l(src).dst_i,
+                    src,
+                    dst,
+                };
+                self.steps.push_front(PathStep::Turn(turn));
+                self.total_length += map.get_t(turn).geom.length();
+            }
+            _ => unreachable!(),
+        }
+        for step in other.steps.into_iter().rev() {
+            self.steps.push_front(step);
+        }
+        self.total_length += other.total_length;
+        self.total_lanes += other.total_lanes;
+    }
+
+    fn append(&mut self, other: Path, map: &Map) {
+        match (*self.steps.back().unwrap(), other.steps[0]) {
+            (PathStep::Lane(src), PathStep::Lane(dst)) => {
+                let turn = TurnID {
+                    parent: map.get_l(src).dst_i,
+                    src,
+                    dst,
+                };
+                self.steps.push_back(PathStep::Turn(turn));
+                self.total_length += map.get_t(turn).geom.length();
+            }
+            _ => unreachable!(),
+        }
+        self.steps.extend(other.steps);
+        self.total_length += other.total_length;
+        self.total_lanes += other.total_lanes;
+    }
 }
 
 // Who's asking for a path?
@@ -507,73 +546,139 @@ impl Pathfinder {
         self.walking_with_transit_graph = Some(SidewalkPathfinder::new(map, true, &self.bus_graph));
     }
 
-    pub fn pathfind(&self, mut req: PathRequest, map: &Map) -> Option<Path> {
+    pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option<Path> {
-        let orig_end_dist = req.end.dist_along();
+        // 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());
 
-        let prepend = if start_r.is_private() {
+        if start_r.is_private() && end_r.is_private() {
+            if req.constraints == PathConstraints::Pedestrian {
+                return None;
+            }
+            let zone1 = map.road_to_zone(start_r.id);
+            let zone2 = map.road_to_zone(end_r.id);
+            if zone1.id == zone2.id {
+                zone1.pathfind(req, map)
+            } else {
+                // TODO Handle paths going between two different zones
+                None
+            }
+        } else if start_r.is_private() {
+            if req.constraints == PathConstraints::Pedestrian {
+                return None;
+            }
             let zone = map.road_to_zone(start_r.id);
-            let (path, connection) = zone.find_path(&req, false, map)?;
+            let mut borders: Vec<&Intersection> =
-            req.start = Position::new(connection, Distance::ZERO);
+                zone.borders.iter().map(|i| map.get_i(*i)).collect();
-            Some(path)
+            // TODO Use the CH to pick the lowest overall cost?
-        } else {
+            let pt = req.end.pt(map);
-            None
+            borders.sort_by_key(|i| pt.dist_to(i.polygon.center()));
-        };
-        let append = if end_r.is_private() {
-            let zone = map.road_to_zone(end_r.id);
-            let (path, connection) = zone.find_path(&req, true, map)?;
-            req.end = Position::new(connection, map.get_l(connection).length());
-            Some(path)
-        } else {
-            None
-        };
 
+            for i in borders {
+                if let Some(result) = self.pathfind_from_zone(i, req.clone(), zone, map) {
+                    validate_continuity(map, &result.steps.iter().cloned().collect());
+                    return Some(result);
+                }
+            }
+            None
+        } else if end_r.is_private() {
+            if req.constraints == PathConstraints::Pedestrian {
+                return None;
+            }
+            let zone = map.road_to_zone(end_r.id);
+            let mut borders: Vec<&Intersection> =
+                zone.borders.iter().map(|i| map.get_i(*i)).collect();
+            // TODO Use the CH to pick the lowest overall cost?
+            let pt = req.start.pt(map);
+            borders.sort_by_key(|i| pt.dist_to(i.polygon.center()));
+
+            for i in borders {
+                if let Some(result) = self.pathfind_to_zone(i, req.clone(), zone, map) {
+                    validate_continuity(map, &result.steps.iter().cloned().collect());
+                    return Some(result);
+                }
+            }
+            None
+        } else {
+            match req.constraints {
+                PathConstraints::Pedestrian => self.walking_graph.pathfind(&req, map),
+                PathConstraints::Car => self.car_graph.pathfind(&req, map).map(|(p, _)| p),
+                PathConstraints::Bike => self.bike_graph.pathfind(&req, map).map(|(p, _)| p),
+                PathConstraints::Bus => self.bus_graph.pathfind(&req, map).map(|(p, _)| p),
+            }
+        }
+    }
+
+    fn pathfind_from_zone(
+        &self,
+        i: &Intersection,
+        mut req: PathRequest,
+        zone: &Zone,
+        map: &Map,
+    ) -> Option<Path> {
+        // TODO Should probably try all combos of incoming/outgoing lanes per border, but I think
+        // generally one should suffice?
+        let src = i
+            .get_incoming_lanes(map, req.constraints)
+            .find(|l| zone.members.contains(&map.get_l(*l).parent))?;
+        let dst = i
+            .get_outgoing_lanes(map, req.constraints)
+            .into_iter()
+            .find(|l| !zone.members.contains(&map.get_l(*l).parent))?;
+        let interior_path = zone.pathfind(
+            PathRequest {
+                start: req.start,
+                end: Position::new(src, map.get_l(src).length()),
+                constraints: req.constraints,
+            },
+            map,
+        )?;
+        req.start = Position::new(dst, Distance::ZERO);
         let mut main_path = match req.constraints {
             PathConstraints::Pedestrian => self.walking_graph.pathfind(&req, map),
             PathConstraints::Car => self.car_graph.pathfind(&req, map).map(|(p, _)| p),
             PathConstraints::Bike => self.bike_graph.pathfind(&req, map).map(|(p, _)| p),
             PathConstraints::Bus => self.bus_graph.pathfind(&req, map).map(|(p, _)| p),
         }?;
+        main_path.prepend(interior_path, map);
+        Some(main_path)
+    }
 
-        if let Some(p) = prepend {
+    fn pathfind_to_zone(
-            match (*p.steps.back().unwrap(), main_path.steps[0]) {
+        &self,
-                (PathStep::Lane(src), PathStep::Lane(dst)) => {
+        i: &Intersection,
-                    let turn = TurnID {
+        mut req: PathRequest,
-                        parent: map.get_l(src).dst_i,
+        zone: &Zone,
-                        src,
+        map: &Map,
-                        dst,
+    ) -> Option<Path> {
-                    };
+        // TODO Should probably try all combos of incoming/outgoing lanes per border, but I think
-                    main_path.steps.push_front(PathStep::Turn(turn));
+        // generally one should suffice?
-                    main_path.total_length += map.get_t(turn).geom.length();
+        let src = i
-                }
+            .get_incoming_lanes(map, req.constraints)
-                _ => unreachable!(),
+            .find(|l| !zone.members.contains(&map.get_l(*l).parent))?;
-            }
+        let dst = i
-            for step in p.steps.into_iter().rev() {
+            .get_outgoing_lanes(map, req.constraints)
-                main_path.steps.push_front(step);
+            .into_iter()
-            }
+            .find(|l| zone.members.contains(&map.get_l(*l).parent))?;
-            main_path.total_length += p.total_length;
+        let interior_path = zone.pathfind(
-            main_path.total_lanes += p.total_lanes;
+            PathRequest {
-        }
+                start: Position::new(dst, Distance::ZERO),
-        if let Some(p) = append {
+                end: req.end,
-            match (*main_path.steps.back().unwrap(), p.steps[0]) {
+                constraints: req.constraints,
-                (PathStep::Lane(src), PathStep::Lane(dst)) => {
+            },
-                    let turn = TurnID {
+            map,
-                        parent: map.get_l(src).dst_i,
+        )?;
-                        src,
+        let orig_end_dist = req.end.dist_along();
-                        dst,
+        req.end = Position::new(src, map.get_l(src).length());
-                    };
+        let mut main_path = match req.constraints {
-                    main_path.steps.push_back(PathStep::Turn(turn));
+            PathConstraints::Pedestrian => self.walking_graph.pathfind(&req, map),
-                    main_path.total_length += map.get_t(turn).geom.length();
+            PathConstraints::Car => self.car_graph.pathfind(&req, map).map(|(p, _)| p),
-                }
+            PathConstraints::Bike => self.bike_graph.pathfind(&req, map).map(|(p, _)| p),
-                _ => unreachable!(),
+            PathConstraints::Bus => self.bus_graph.pathfind(&req, map).map(|(p, _)| p),
-            }
+        }?;
-            main_path.steps.extend(p.steps);
+        main_path.append(interior_path, map);
-            main_path.total_length += p.total_length;
+        main_path.end_dist = orig_end_dist;
-            main_path.total_lanes += p.total_lanes;
-            main_path.end_dist = orig_end_dist;
-        }
-        validate_continuity(map, &main_path.steps.iter().cloned().collect());
         Some(main_path)
     }
 

map_model/src/zone.rs

@@ -1,9 +1,5 @@
 use crate::pathfind::cost;
-use crate::{
+use crate::{IntersectionID, LaneID, Map, Path, PathRequest, PathStep, RoadID, TurnID};
-    Intersection, IntersectionID, LaneID, Map, Path, PathRequest, PathStep, Position, RoadID,
-    TurnID,
-};
-use geom::Distance;
 use petgraph::graphmap::DiGraphMap;
 use serde::{Deserialize, Serialize};
 use std::collections::BTreeSet;
@@ -27,57 +23,8 @@ pub struct Zone {
 }
 
 impl Zone {
-    // The lane connection returned is the last lane outside the zone if entering, or the first
-    // lane outside the zone if exiting.
-    pub(crate) fn find_path(
-        &self,
-        req: &PathRequest,
-        entering: bool,
-        map: &Map,
-    ) -> Option<(Path, LaneID)> {
-        let mut borders: Vec<&Intersection> = self.borders.iter().map(|i| map.get_i(*i)).collect();
-        // TODO Use the CH
-        let pt = if entering { req.start } else { req.end }.pt(map);
-        borders.sort_by_key(|i| pt.dist_to(i.polygon.center()));
-
-        for i in borders {
-            // TODO Should probably try all combos of incoming/outgoing lanes per border, but I
-            // think generally one should suffice?
-            if let Some(src) = i
-                .get_incoming_lanes(map, req.constraints)
-                .find(|l| self.members.contains(&map.get_l(*l).parent) == !entering)
-            {
-                if let Some(dst) = i
-                    .get_outgoing_lanes(map, req.constraints)
-                    .into_iter()
-                    .find(|l| self.members.contains(&map.get_l(*l).parent) == entering)
-                {
-                    if let Some(interior_path) = self.pathfind(
-                        PathRequest {
-                            start: if entering {
-                                Position::new(dst, Distance::ZERO)
-                            } else {
-                                req.start
-                            },
-                            end: if entering {
-                                req.end
-                            } else {
-                                Position::new(src, map.get_l(src).length())
-                            },
-                            constraints: req.constraints,
-                        },
-                        map,
-                    ) {
-                        return Some((interior_path, if entering { src } else { dst }));
-                    }
-                }
-            }
-        }
-        None
-    }
-
     // Run slower Dijkstra's within the interior of a private zone. Don't go outside the borders.
-    fn pathfind(&self, req: PathRequest, map: &Map) -> Option<Path> {
+    pub(crate) fn pathfind(&self, req: PathRequest, map: &Map) -> Option<Path> {
         // Edge type is the Turn, but we don't need it
         let mut graph: DiGraphMap<LaneID, TurnID> = DiGraphMap::new();
         for r in &self.members {