start two-step pathfinding for starting/ending in private zones. not

handling interior trips or walking yet. failing to find a path in many
cases, but it's a first step.

map_model/src/map.rs

@@ -745,6 +745,11 @@ impl Map {
         None
     }
 
+    pub fn road_to_zone(&self, r: RoadID) -> &Zone {
+        // TODO Consider indexing
+        self.zones.iter().find(|z| z.members.contains(&r)).unwrap()
+    }
+
     pub fn right_shift(&self, pl: PolyLine, width: Distance) -> Warn<PolyLine> {
         self.driving_side.right_shift(pl, width)
     }

map_model/src/pathfind/mod.rs

@@ -402,7 +402,7 @@ impl fmt::Display for PathRequest {
 
 fn validate_continuity(map: &Map, steps: &Vec<PathStep>) {
     if steps.is_empty() {
-        panic!("Empty Path");
+        panic!("Empty path");
     }
     for pair in steps.windows(2) {
         let from = match pair[0] {
@@ -507,13 +507,90 @@ impl Pathfinder {
         self.walking_with_transit_graph = Some(SidewalkPathfinder::new(map, true, &self.bus_graph));
     }
 
-    pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option<Path> {
-        match req.constraints {
+    pub fn pathfind(&self, mut req: PathRequest, map: &Map) -> Option<Path> {
+        let orig_end_dist = req.end.dist_along();
+        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() {
+            let zone = map.road_to_zone(start_r.id);
+            let (src, dst) = zone.find_border(req.end, false, req.constraints, map)?;
+            let result = 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);
+            Some(result)
+        } else {
+            None
+        };
+        let append = if end_r.is_private() {
+            let zone = map.road_to_zone(end_r.id);
+            let (src, dst) = zone.find_border(req.start, true, req.constraints, map)?;
+            let result = zone.pathfind(
+                PathRequest {
+                    start: Position::new(dst, Distance::ZERO),
+                    end: req.end,
+                    constraints: req.constraints,
+                },
+                map,
+            )?;
+            req.end = Position::new(src, Distance::ZERO);
+            Some(result)
+        } else {
+            None
+        };
+
+        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),
+        }?;
+
+        if let Some(p) = prepend {
+            match (*p.steps.back().unwrap(), main_path.steps[0]) {
+                (PathStep::Lane(src), PathStep::Lane(dst)) => {
+                    let turn = TurnID {
+                        parent: map.get_l(src).dst_i,
+                        src,
+                        dst,
+                    };
+                    main_path.steps.push_front(PathStep::Turn(turn));
+                    main_path.total_length += map.get_t(turn).geom.length();
+                }
+                _ => unreachable!(),
+            }
+            for step in p.steps.into_iter().rev() {
+                main_path.steps.push_front(step);
+            }
+            main_path.total_length += p.total_length;
+            main_path.total_lanes += p.total_lanes;
         }
+        if let Some(p) = append {
+            match (*main_path.steps.back().unwrap(), p.steps[0]) {
+                (PathStep::Lane(src), PathStep::Lane(dst)) => {
+                    let turn = TurnID {
+                        parent: map.get_l(src).dst_i,
+                        src,
+                        dst,
+                    };
+                    main_path.steps.push_back(PathStep::Turn(turn));
+                    main_path.total_length += map.get_t(turn).geom.length();
+                }
+                _ => unreachable!(),
+            }
+            main_path.steps.extend(p.steps);
+            main_path.total_length += p.total_length;
+            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)
     }
 
     pub fn should_use_transit(

map_model/src/zone.rs

@@ -1,4 +1,9 @@
-use crate::{IntersectionID, RoadID};
+use crate::pathfind::cost;
+use crate::{
+    Intersection, IntersectionID, LaneID, Map, Path, PathConstraints, PathRequest, PathStep,
+    Position, RoadID, TurnID,
+};
+use petgraph::graphmap::DiGraphMap;
 use serde::{Deserialize, Serialize};
 use std::collections::BTreeSet;
 use std::fmt;
@@ -19,3 +24,69 @@ pub struct Zone {
     pub members: BTreeSet<RoadID>,
     pub borders: BTreeSet<IntersectionID>,
 }
+
+impl Zone {
+    // If entering, then finds the (last lane outside the zone, first lane inside the zone). If
+    // not, the opposite.
+    pub(crate) fn find_border(
+        &self,
+        outside: Position,
+        entering: bool,
+        constraints: PathConstraints,
+        map: &Map,
+    ) -> Option<(LaneID, LaneID)> {
+        let mut borders: Vec<&Intersection> = self.borders.iter().map(|i| map.get_i(*i)).collect();
+        // TODO Use the CH
+        let pt = outside.pt(map);
+        borders.sort_by_key(|i| pt.dist_to(i.polygon.center()));
+
+        for i in borders {
+            let src = i
+                .get_incoming_lanes(map, constraints)
+                .find(|l| self.members.contains(&map.get_l(*l).parent) == !entering)?;
+            let dst = i
+                .get_outgoing_lanes(map, constraints)
+                .into_iter()
+                .find(|l| self.members.contains(&map.get_l(*l).parent) == entering)?;
+            return Some((src, dst));
+        }
+        None
+    }
+
+    // Run slower Dijkstra's within the interior of a private zone. Don't go outside the borders.
+    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 {
+            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)| cost(map.get_l(turn.src), map.get_t(*turn), req.constraints, map),
+            |_| 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()));
+        Some(Path::new(map, steps, req.end.dist_along()))
+    }
+}