Create static blockages for off-side driveway exits. Verified correct

for all combinations of on/off-side exits and cutting across lanes by
using the static blockage visualizer.

map_model/src/pathfind/v1.rs

@@ -98,7 +98,7 @@ pub struct Path {
     // Is the current_step in the middle of an UberTurn?
     currently_inside_ut: Option<UberTurn>,
 
-    blocked_starts: Vec<Position>,
+    blocked_starts: Vec<LaneID>,
 }
 
 impl Path {
@@ -107,7 +107,7 @@ impl Path {
         steps: Vec<PathStep>,
         orig_req: PathRequest,
         uber_turns: Vec<UberTurn>,
-        blocked_starts: Vec<Position>,
+        blocked_starts: Vec<LaneID>,
     ) -> Path {
         // Haven't seen problems here in a very long time. Noticeably saves some time to skip.
         if false {
@@ -422,9 +422,9 @@ impl Path {
     }
 
     /// If the agent following this path will initially block some intermediate lanes as they move
-    /// between a driveway and `get_req().start`, then record those equivalent positions here.
-    pub fn get_blocked_starts(&self) -> &Vec<Position> {
-        &self.blocked_starts
+    /// between a driveway and `get_req().start`, then record them here.
+    pub fn get_blocked_starts(&self) -> Vec<LaneID> {
+        self.blocked_starts.clone()
     }
 }
 

map_model/src/pathfind/v2.rs

@@ -100,6 +100,7 @@ impl PathV2 {
         }
 
         // If we had two possible start positions, figure out which one we wound up using
+        let orig_start_lane = self.req.start.lane();
         if let Some((pos, _)) = self.req.alt_start {
             if let PathStepV2::Along(dr) = self.steps[0] {
                 if map.get_l(self.req.start.lane()).get_directed_parent() == dr {
@@ -210,15 +211,15 @@ impl PathV2 {
                 }
                 steps.push(PathStep::Lane(self.req.end.lane()));
                 let mut blocked_starts = Vec::new();
-                if steps[0] != PathStep::Lane(self.req.start.lane()) {
+                if steps[0] != PathStep::Lane(orig_start_lane) {
                     let actual_start = match steps[0] {
                         PathStep::Lane(l) => l,
                         _ => unreachable!(),
                     };
-                    blocked_starts.push(self.req.start);
-                    for l in start_road.get_lanes_between(start_lane, actual_start) {
-                        blocked_starts.push(self.req.start.equiv_pos(l, map));
-                    }
+                    blocked_starts.push(orig_start_lane);
+                    blocked_starts
+                        .extend(start_road.get_lanes_between(orig_start_lane, actual_start));
+                    // Sometimes a no-op for exiting off-side
                     self.req.start = self.req.start.equiv_pos(actual_start, map);
                 }
                 let uber_turns = find_uber_turns(&steps, map, self.uber_turns);

sim/src/mechanics/driving.rs

@@ -116,17 +116,25 @@ impl DrivingSimState {
             }
         }
 
-        // First handle any of the intermediate queues, failing fast.
-        let mut blocked_start_indices = Vec::new();
-        for pos in params.router.get_path().get_blocked_starts() {
-            if let Some(idx) = self.queues[&Traversable::Lane(pos.lane())].can_block_from_driveway(
-                pos,
+        // First handle any of the intermediate queues, failing fast. Record the position of the
+        // blockage's front and the index in that queue.
+        let mut blocked_starts: Vec<(Position, usize)> = Vec::new();
+        for lane in params.router.get_path().get_blocked_starts() {
+            let pos = params
+                .router
+                .get_path()
+                .get_req()
+                .start
+                .equiv_pos(lane, ctx.map);
+            if let Some(idx) = self.queues[&Traversable::Lane(lane)].can_block_from_driveway(
+                // This is before adjusting for the length of the vehicle exiting the driveway
+                &pos,
                 params.vehicle.length,
                 now,
                 &self.cars,
                 &self.queues,
             ) {
-                blocked_start_indices.push(idx);
+                blocked_starts.push((pos, idx));
             } else {
                 return Some(params);
             }
@@ -170,7 +178,7 @@ impl DrivingSimState {
                         //
                         // TODO Actually, revisit ~instantaneous unparking in infinite mode. Were
                         // we making gridlock progress somewhere because of this?
-                        if car.router.get_path().get_blocked_starts().is_empty() {
+                        if blocked_starts.is_empty() {
                             self.time_to_unpark_offstreet
                         } else {
                             Duration::seconds(5.0)
@@ -180,20 +188,14 @@ impl DrivingSimState {
                 car.state =
                     CarState::Unparking(start_dist, p.spot, TimeInterval::new(now, now + delay));
 
-                for (pos, idx) in car
-                    .router
-                    .get_path()
-                    .get_blocked_starts()
-                    .iter()
-                    .zip(blocked_start_indices)
-                {
+                for (pos, idx) in blocked_starts {
                     self.queues
                         .get_mut(&Traversable::Lane(pos.lane()))
                         .unwrap()
                         .add_static_blockage(
                             car.vehicle.id,
-                            start_dist,
-                            start_dist - car.vehicle.length,
+                            pos.dist_along(),
+                            pos.dist_along() - car.vehicle.length,
                             idx,
                         );
                 }
@@ -389,12 +391,12 @@ impl DrivingSimState {
                 }
             }
             CarState::Unparking(front, _, _) => {
-                for pos in car.router.get_path().get_blocked_starts() {
+                for lane in car.router.get_path().get_blocked_starts() {
                     // Calculate the exact positions along this blocked queue (which is ***NOT***
                     // the same queue that the unparking car is in!). Use that to update the
                     // follower. Note that it's fine that the current car isn't currently in
                     // self.cars; the static blockage doesn't need it.
-                    let dists = self.queues[&Traversable::Lane(pos.lane())].get_car_positions(
+                    let dists = self.queues[&Traversable::Lane(lane)].get_car_positions(
                         now,
                         &self.cars,
                         &self.queues,
@@ -403,7 +405,7 @@ impl DrivingSimState {
                     self.update_follower(idx, &dists, now, ctx);
 
                     self.queues
-                        .get_mut(&Traversable::Lane(pos.lane()))
+                        .get_mut(&Traversable::Lane(lane))
                         .unwrap()
                         .clear_static_blockage(car.vehicle.id, idx);
                 }

sim/src/mechanics/queue.rs

@@ -511,7 +511,8 @@ impl Queue {
     }
 
     /// True if a static blockage can be inserted into the queue without anything already there
-    /// intersecting it. Returns the index if so.
+    /// intersecting it. Returns the index if so. The position represents the front of the
+    /// blockage.
     pub fn can_block_from_driveway(
         &self,
         pos: &Position,