Implement dynamic lane-changing in a simple way:

- only triggered when a vehicle becomes Queued
- Only one adjacent lane, no contraflow (crossing the road's center
  line)
- Don't return to the original lane after passing
- Using a static blockage in the old lane (so other vehicles will wait
  too much)
- Only using the new lane to determine position (so visually a car will
  clip a bike as they pass)

Haven't regenerated prebaked data. #382

map_model/src/pathfind/v1.rs

@@ -269,7 +269,6 @@ impl Path {
     /// Trusting the caller to do this in valid ways.
     pub fn modify_step(&mut self, idx: usize, step: PathStep, map: &Map) {
         assert!(self.currently_inside_ut.is_none());
-        assert!(idx != 0);
         // We're assuming this step was in the middle of the path, meaning we were planning to
         // travel its full length
         self.total_length -= self.steps[idx].as_traversable().get_polyline(map).length();

sim/src/mechanics/car.rs

@@ -3,7 +3,7 @@ use std::collections::{BTreeSet, VecDeque};
 use serde::{Deserialize, Serialize};
 
 use geom::{Distance, Duration, PolyLine, Time, EPSILON_DIST};
-use map_model::{Direction, Map, Traversable};
+use map_model::{Direction, LaneID, Map, Traversable};
 
 use crate::{
     CarID, CarStatus, DistanceInterval, DrawCarInput, ParkingSpot, PersonID, Router, TimeInterval,
@@ -133,6 +133,33 @@ impl Car {
         };
 
         let body = match self.state {
+            CarState::ChangingLanes {
+                from,
+                to,
+                ref lc_time,
+                ..
+            } => {
+                let percent_time = 1.0 - lc_time.percent(now);
+                // TODO Can probably simplify this! Lifted from the parking case
+                let r = map.get_parent(from);
+                // The car's body is already at 'to', so shift back
+                let mut diff = (r.offset(to) as isize) - (r.offset(from) as isize);
+                if map.get_l(from).dir == Direction::Fwd {
+                    diff *= -1;
+                }
+                // TODO Careful with this width math
+                let width = map.get_l(from).width * (diff as f64) * percent_time;
+                match raw_body.shift_right(width) {
+                    Ok(pl) => pl,
+                    Err(err) => {
+                        println!(
+                            "Body for lane-changing {} at {} broken: {}",
+                            self.vehicle.id, now, err
+                        );
+                        raw_body
+                    }
+                }
+            }
             CarState::Unparking(_, ref spot, ref time_int)
             | CarState::Parking(_, ref spot, ref time_int) => {
                 let (percent_time, is_parking) = match self.state {
@@ -232,6 +259,7 @@ impl Car {
                 CarState::Queued { .. } => CarStatus::Moving,
                 CarState::WaitingToAdvance { .. } => CarStatus::Moving,
                 CarState::Crossing(_, _) => CarStatus::Moving,
+                CarState::ChangingLanes { .. } => CarStatus::Moving,
                 CarState::Unparking(_, _, _) => CarStatus::Moving,
                 CarState::Parking(_, _, _) => CarStatus::Moving,
                 // Changing color for idling buses is helpful
@@ -272,8 +300,18 @@ impl Car {
 #[derive(Debug, Serialize, Deserialize, Clone)]
 pub(crate) enum CarState {
     Crossing(TimeInterval, DistanceInterval),
+    ChangingLanes {
+        from: LaneID,
+        to: LaneID,
+        // For the most part, act just like a Crossing state with these intervals
+        new_time: TimeInterval,
+        new_dist: DistanceInterval,
+        // How long does the lane-changing itself last? This must end before new_time_int does.
+        lc_time: TimeInterval,
+    },
     Queued {
         blocked_since: Time,
+        want_to_change_lanes: Option<LaneID>,
     },
     WaitingToAdvance {
         blocked_since: Time,
@@ -290,6 +328,8 @@ impl CarState {
             CarState::Crossing(ref time_int, _) => time_int.end,
             CarState::Queued { .. } => unreachable!(),
             CarState::WaitingToAdvance { .. } => unreachable!(),
+            // Note this state lasts for lc_time, NOT for new_time.
+            CarState::ChangingLanes { ref lc_time, .. } => lc_time.end,
             CarState::Unparking(_, _, ref time_int) => time_int.end,
             CarState::Parking(_, _, ref time_int) => time_int.end,
             CarState::IdlingAtStop(_, ref time_int) => time_int.end,
@@ -298,9 +338,8 @@ impl CarState {
 
     pub fn time_spent_waiting(&self, now: Time) -> Duration {
         match self {
-            CarState::Queued { blocked_since } | CarState::WaitingToAdvance { blocked_since } => {
+            CarState::Queued { blocked_since, .. }
-                now - *blocked_since
+            | CarState::WaitingToAdvance { blocked_since } => now - *blocked_since,
-            }
             _ => Duration::ZERO,
         }
     }

sim/src/mechanics/driving.rs

@@ -4,7 +4,7 @@ use serde::{Deserialize, Serialize};
 
 use abstutil::{deserialize_hashmap, serialize_hashmap, FixedMap, IndexableKey};
 use geom::{Distance, Duration, PolyLine, Time};
-use map_model::{IntersectionID, LaneID, Map, Path, Position, Traversable};
+use map_model::{DrivingSide, IntersectionID, LaneID, Map, Path, Position, Traversable};
 
 use crate::mechanics::car::{Car, CarState};
 use crate::mechanics::queue::{Queue, QueueEntry, Queued};
@@ -17,6 +17,7 @@ use crate::{
 };
 
 const TIME_TO_WAIT_AT_BUS_STOP: Duration = Duration::const_seconds(10.0);
+const TIME_TO_CHANGE_LANES: Duration = Duration::const_seconds(1.0);
 
 // TODO Do something else.
 pub const BLIND_RETRY_TO_CREEP_FORWARDS: Duration = Duration::const_seconds(0.1);
@@ -133,7 +134,10 @@ impl DrivingSimState {
                 vehicle: params.vehicle,
                 router: params.router,
                 // Temporary
-                state: CarState::Queued { blocked_since: now },
+                state: CarState::Queued {
+                    blocked_since: now,
+                    want_to_change_lanes: None,
+                },
                 last_steps: VecDeque::new(),
                 started_at: now,
                 total_blocked_time: Duration::ZERO,
@@ -320,7 +324,10 @@ impl DrivingSimState {
     ) -> bool {
         match car.state {
             CarState::Crossing(_, _) => {
-                car.state = CarState::Queued { blocked_since: now };
+                car.state = CarState::Queued {
+                    blocked_since: now,
+                    want_to_change_lanes: None,
+                };
                 if car.router.last_step() {
                     // Immediately run update_car_with_distances.
                     return true;
@@ -351,6 +358,17 @@ impl DrivingSimState {
                             Problem::OvertakeDesired(queue.id),
                         ));
                     }
+
+                    if let Some(target_lane) = self.pick_overtaking_lane(car, ctx.map) {
+                        // We need the current position of the car to see if lane-changing is
+                        // actually feasible right now, so record our intention and trigger
+                        // update_car_with_distances.
+                        car.state = CarState::Queued {
+                            blocked_since: now,
+                            want_to_change_lanes: Some(target_lane),
+                        };
+                        return true;
+                    }
                 }
             }
             CarState::Unparking(front, _, _) => {
@@ -478,6 +496,33 @@ impl DrivingSimState {
                     .unwrap()
                     .push_car_onto_end(car.vehicle.id);
             }
+            CarState::ChangingLanes {
+                from,
+                new_time,
+                new_dist,
+                ..
+            } => {
+                // The car is already in the target queue. Just set them in the crossing state; we
+                // already calculated the intervals for it.
+                car.state = CarState::Crossing(new_time, new_dist);
+                ctx.scheduler
+                    .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
+
+                // And remove the blockage from the old queue. Similar to the note in this function
+                // for Unparking, we calculate distances in that OTHER queue.
+                let dists = self.queues[&Traversable::Lane(from)].get_car_positions(
+                    now,
+                    &self.cars,
+                    &self.queues,
+                );
+                let idx = dists.iter().position(|entry| matches!(entry.member, Queued::StaticBlockage { cause, ..} if cause == car.vehicle.id)).unwrap();
+                self.update_follower(idx, &dists, now, ctx);
+
+                self.queues
+                    .get_mut(&Traversable::Lane(from))
+                    .unwrap()
+                    .clear_blockage(car.vehicle.id, idx);
+            }
             CarState::Queued { .. } => unreachable!(),
             CarState::Parking(_, _, _) => unreachable!(),
             CarState::IdlingAtStop(_, _) => unreachable!(),
@@ -502,8 +547,19 @@ impl DrivingSimState {
         match car.state {
             CarState::Crossing(_, _)
             | CarState::Unparking(_, _, _)
-            | CarState::WaitingToAdvance { .. } => unreachable!(),
+            | CarState::WaitingToAdvance { .. }
-            CarState::Queued { blocked_since } => {
+            | CarState::ChangingLanes { .. } => unreachable!(),
+            CarState::Queued {
+                blocked_since,
+                want_to_change_lanes,
+            } => {
+                // Two totally different reasons we'll wind up here: we want to lane-change, and
+                // we're on our last step.
+                if let Some(target_lane) = want_to_change_lanes {
+                    self.try_start_lc(car, our_dist, idx, target_lane, now, ctx);
+                    return true;
+                }
+
                 match car.router.maybe_handle_end(
                     our_dist,
                     &car.vehicle,
@@ -758,7 +814,7 @@ impl DrivingSimState {
             // car's back is still sticking out. Need to still be bound by them, even though they
             // don't exist! If the leader just parked, then we're fine.
             match follower.state {
-                CarState::Queued { blocked_since } => {
+                CarState::Queued { blocked_since, .. } => {
                     // TODO If they're on their last step, they might be ending early and not right
                     // behind us? !follower.router.last_step()
 
@@ -780,6 +836,33 @@ impl DrivingSimState {
                         Command::UpdateCar(follower_id),
                     );
                 }
+                CarState::ChangingLanes {
+                    from, to, lc_time, ..
+                } => {
+                    // This is a fun case -- something stopped blocking somebody that was in the
+                    // process of lane-changing! Similar to the Crossing case above, we just have
+                    // to update the distance/time intervals, but otherwise leave them in the
+                    // middle of their lane-changing. It's guaranteed that lc_time will continue to
+                    // finish before the new time interval, because there's no possible way
+                    // recalculating this crossing state here will speed things up from the
+                    // original estimate.
+                    let (new_time, new_dist) = match follower.crossing_state_with_end_dist(
+                        DistanceInterval::new_driving(follower_dist, ctx.map.get_l(to).length()),
+                        now,
+                        ctx.map,
+                    ) {
+                        CarState::Crossing(time, dist) => (time, dist),
+                        _ => unreachable!(),
+                    };
+                    assert!(new_time.end >= lc_time.end);
+                    follower.state = CarState::ChangingLanes {
+                        from,
+                        to,
+                        new_time,
+                        new_dist,
+                        lc_time,
+                    };
+                }
                 // They weren't blocked
                 CarState::Unparking(_, _, _)
                 | CarState::Parking(_, _, _)
@@ -895,7 +978,7 @@ impl DrivingSimState {
                         let mut follower = self.cars.get_mut(follower_id).unwrap();
 
                         match follower.state {
-                            CarState::Queued { blocked_since } => {
+                            CarState::Queued { blocked_since, .. } => {
                                 // If they're on their last step, they might be ending early and not
                                 // right behind us.
                                 if !follower.router.last_step() {
@@ -919,6 +1002,7 @@ impl DrivingSimState {
                             // They weren't blocked. Note that there's no way the Crossing state could
                             // jump forwards here; the leader vanished from the end of the traversable.
                             CarState::Crossing(_, _)
+                            | CarState::ChangingLanes { .. }
                             | CarState::Unparking(_, _, _)
                             | CarState::Parking(_, _, _)
                             | CarState::IdlingAtStop(_, _) => {}
@@ -933,6 +1017,143 @@ impl DrivingSimState {
         }
     }
 
+    /// If the car wants to over-take somebody, what adjacent lane should they use?
+    /// - The lane must be in the same direction as the current; no support for crossing the road's
+    ///   yellow line yet.
+    /// - Prefer passing on the left (for DrivingSide::Right)
+    /// For now, just pick one candidate lane, even if both might be usable.
+    fn pick_overtaking_lane(&self, car: &Car, map: &Map) -> Option<LaneID> {
+        // Don't overtake in the middle of a turn!
+        let current_lane = map.get_l(car.router.head().maybe_lane()?);
+        let road = map.get_r(current_lane.parent);
+        let idx = road.offset(current_lane.id);
+        let lanes_ltr = road.lanes_ltr();
+
+        let mut candidates = Vec::new();
+        if idx != 0 {
+            candidates.push(lanes_ltr[idx - 1].0);
+        }
+        if idx != lanes_ltr.len() - 1 {
+            candidates.push(lanes_ltr[idx + 1].0);
+        }
+        if map.get_config().driving_side == DrivingSide::Left {
+            candidates.reverse();
+        }
+
+        for l in candidates {
+            let target_lane = map.get_l(l);
+            // Must be the same direction -- no crossing into oncoming traffic yet
+            if current_lane.dir != target_lane.dir {
+                continue;
+            }
+            // The lane types can differ, as long as the vehicle can use the target. Imagine
+            // overtaking a slower cyclist in a bike lane using the rest of the road.
+            if !car
+                .vehicle
+                .vehicle_type
+                .to_constraints()
+                .can_use(target_lane, map)
+            {
+                continue;
+            }
+            // Is this other lane compatible with the path? We won't make any attempts to return to the
+            // original lane after changing.
+            if !car
+                .router
+                .can_lanechange(current_lane.id, target_lane.id, map)
+            {
+                continue;
+            }
+            return Some(target_lane.id);
+        }
+
+        None
+    }
+
+    fn try_start_lc(
+        &mut self,
+        car: &mut Car,
+        front_current_queue: Distance,
+        idx_in_current_queue: usize,
+        target_lane: LaneID,
+        now: Time,
+        ctx: &mut Ctx,
+    ) {
+        // If the lanes are very different lengths and we're too close to the end at the target,
+        // not going to work.
+        if front_current_queue >= ctx.map.get_l(target_lane).length() {
+            return;
+        }
+        let current_lane = car.router.head().as_lane();
+        let front_target_queue = Position::new(current_lane, front_current_queue)
+            .equiv_pos(target_lane, ctx.map)
+            .dist_along();
+
+        // Calculate the crossing state in the target queue. Pass in the DistanceInterval
+        // explicitly, because we haven't modified the route yet.
+        let (new_time, new_dist) = match car.crossing_state_with_end_dist(
+            DistanceInterval::new_driving(front_target_queue, ctx.map.get_l(target_lane).length()),
+            now,
+            ctx.map,
+        ) {
+            CarState::Crossing(time, dist) => (time, dist),
+            _ => unreachable!(),
+        };
+
+        // Do we have enough time to finish the lane-change, assuming that we go as fast as
+        // possible in the target?
+        let lc_time = TimeInterval::new(now, now + TIME_TO_CHANGE_LANES);
+        if lc_time.end >= new_time.end {
+            return;
+        }
+
+        // Is there room for us to sliiiide on over into that lane's DMs?
+        if let Some(idx_in_target_queue) = self.queues[&Traversable::Lane(target_lane)]
+            .get_idx_to_insert_car(
+                front_target_queue,
+                car.vehicle.length,
+                now,
+                &self.cars,
+                &self.queues,
+            )
+        {
+            // In the current queue, replace with a static blockage that just stays exactly where
+            // the car currently is.
+            {
+                let queue = self.queues.get_mut(&car.router.head()).unwrap();
+                queue.remove_car_from_idx(car.vehicle.id, idx_in_current_queue);
+                queue.free_reserved_space(car);
+                queue.add_blockage(
+                    car.vehicle.id,
+                    front_current_queue,
+                    front_current_queue - car.vehicle.length,
+                    // The same index should work
+                    idx_in_current_queue,
+                );
+            }
+
+            // Change the path
+            car.router.confirm_lanechange(target_lane, ctx.map);
+
+            // Insert into the new queue
+            self.queues
+                .get_mut(&car.router.head())
+                .unwrap()
+                .insert_car_at_idx(idx_in_target_queue, car);
+
+            // Put into the new state
+            car.state = CarState::ChangingLanes {
+                from: current_lane,
+                to: target_lane,
+                new_time,
+                new_dist,
+                lc_time,
+            };
+            ctx.scheduler
+                .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
+        }
+    }
+
     pub fn collect_events(&mut self) -> Vec<Event> {
         std::mem::take(&mut self.events)
     }

sim/src/mechanics/queue.rs

@@ -9,7 +9,7 @@ use map_model::{Map, Position, Traversable};
 use crate::mechanics::car::{Car, CarState};
 use crate::{CarID, VehicleType, FOLLOWING_DISTANCE};
 
-/// A Queue of vehicles on a single lane or turn. No over-taking or lane-changing. This is where
+/// A Queue of vehicles on a single lane or turn. This is where
 /// https://a-b-street.github.io/docs/tech/trafficsim/discrete_event.html#exact-positions is
 /// implemented.
 ///
@@ -224,6 +224,14 @@ impl Queue {
                             // calculate this before moving this car from Crossing to another state.
                             dist_int.lerp(time_int.percent_clamp_end(now)).min(bound)
                         }
+                        CarState::ChangingLanes {
+                            ref new_time,
+                            ref new_dist,
+                            ..
+                        } => {
+                            // Same as the Crossing logic
+                            new_dist.lerp(new_time.percent_clamp_end(now)).min(bound)
+                        }
                         CarState::Unparking(front, _, _) => front,
                         CarState::Parking(front, _, _) => front,
                         CarState::IdlingAtStop(front, _) => front,
@@ -283,8 +291,19 @@ impl Queue {
         // Nope, there's not actually room at the front right now.
         // (This is overly conservative; we could figure out exactly where the laggy head is and
         // maybe allow it.)
-        if self.laggy_head.is_some() && idx == 0 {
+        if idx == 0 {
-            return None;
+            if let Some(c) = self.laggy_head {
+                // We don't know exactly where the laggy head is. So assume the worst case; that
+                // they've just barely started the turn, and we have to use the same
+                // too-close-to-leader math.
+                //
+                // TODO We can be more precise! We already call get_car_positions, and that
+                // calculates exactly where the laggy head is. We just need to plumb that bound
+                // back here.
+                if self.geom_len - cars[&c].vehicle.length - FOLLOWING_DISTANCE < start_dist {
+                    return None;
+                }
+            }
         }
 
         // Are we too close to the leader?
@@ -507,6 +526,16 @@ fn dump_cars(dists: &[QueueEntry], cars: &FixedMap<CarID, Car>, id: Traversable,
                         dist_int.start, dist_int.end, time_int.start, time_int.end
                     );
                 }
+                CarState::ChangingLanes {
+                    ref new_time,
+                    ref new_dist,
+                    ..
+                } => {
+                    println!(
+                        "  Going {} .. {} during {} .. {}, also in the middle of lane-changing",
+                        new_dist.start, new_dist.end, new_time.start, new_time.end
+                    );
+                }
                 CarState::Queued { .. } => {
                     println!("  Queued currently");
                 }

sim/src/router.rs

@@ -203,6 +203,8 @@ impl Router {
         trip_and_person: Option<(TripID, PersonID)>,
         events: &mut Vec<Event>,
     ) -> Option<ActionAtEnd> {
+        assert!(self.path.is_last_step());
+
         match self.goal {
             Goal::EndAtBorder { end_dist, i } => {
                 if end_dist == front {
@@ -468,6 +470,37 @@ impl Router {
         }
     }
 
+    pub fn can_lanechange(&self, from: LaneID, to: LaneID, map: &Map) -> bool {
+        let steps = self.path.get_steps();
+        if steps.len() < 3 {
+            return false;
+        }
+        assert_eq!(PathStep::Lane(from), steps[0]);
+        let current_turn = match steps[1] {
+            PathStep::Turn(t) => t,
+            _ => unreachable!(),
+        };
+        let next_lane = current_turn.dst;
+        assert_eq!(PathStep::Lane(next_lane), steps[2]);
+        map.maybe_get_t(TurnID {
+            parent: current_turn.parent,
+            src: to,
+            dst: next_lane,
+        })
+        .is_some()
+    }
+
+    pub fn confirm_lanechange(&mut self, to: LaneID, map: &Map) {
+        // No assertions, blind trust!
+        self.path.modify_step(0, PathStep::Lane(to), map);
+        let mut turn = match self.path.get_steps()[1] {
+            PathStep::Turn(t) => t,
+            _ => unreachable!(),
+        };
+        turn.src = to;
+        self.path.modify_step(1, PathStep::Turn(turn), map);
+    }
+
     pub fn is_parking(&self) -> bool {
         match self.goal {
             Goal::ParkNearBuilding {