basic, but working, impl of cars lane-changing opportunistically

docs/TODO_quality.md

@@ -89,6 +89,7 @@
 - park/unpark needs to jump two lanes in the case of crossing a bike lane or something
 	- should only be able to park from the closest lane, though!
 - explicit tests making cars park at 0 and max_dist, peds walk to 0 and max_dist
+- lanechange rebalancing
 
 ## Discrete-event sim model
 

docs/articles/trafficsim/article.md

@@ -118,6 +118,11 @@ gridlock. This LCing model was using a detailed discrete-time model with cars
 accelerating properly; maybe it's easier with A/B Street's simplified movement
 model.
 
+Currently in A/B Street, cars will pick the least backed-up lane when there's a
+choice. They make this decision once when they reach the front of a queue; look
+for `opportunistically_lanechange` in `router.rs`. The decision could be
+improved.
+
 ### Pedestrians
 
 Pedestrian modeling -- in `mechanics/walking.rs` is way simpler. Pedestrians

game/src/abtest/setup.rs

@@ -176,6 +176,10 @@ fn launch_test(test: &ABTest, ui: &mut UI, ctx: &mut EventCtx) -> ABTestMode {
                                     .opts
                                     .disable_block_the_box,
                                 record_stats: false,
+                                recalc_lanechanging: current_flags
+                                    .sim_flags
+                                    .opts
+                                    .recalc_lanechanging,
                             },
                         },
                         ..current_flags.clone()

map_model/src/pathfind/mod.rs

@@ -159,6 +159,14 @@ impl Path {
         self.steps.push_back(step);
     }
 
+    // Trusting the caller to do this in valid ways.
+    pub fn modify_step(&mut self, idx: usize, step: PathStep, map: &Map) {
+        assert!(idx != 0);
+        self.total_length -= self.steps[idx].as_traversable().length(map);
+        self.steps[idx] = step;
+        self.total_length += self.steps[idx].as_traversable().length(map);
+    }
+
     pub fn current_step(&self) -> PathStep {
         self.steps[0]
     }
@@ -263,14 +271,6 @@ impl Path {
     pub fn get_steps(&self) -> &VecDeque<PathStep> {
         &self.steps
     }
-
-    pub fn total_dist(&self, map: &Map) -> Distance {
-        let mut dist = Distance::ZERO;
-        for s in &self.steps {
-            dist += s.as_traversable().length(map);
-        }
-        dist
-    }
 }
 
 #[derive(Clone)]

popdat/src/trips.rs

@@ -224,7 +224,7 @@ pub fn clip_trips(map: &Map, timer: &mut Timer) -> (Vec<Trip>, HashMap<BuildingI
                 if false {
                     // TODO Figure out why some paths fail.
                     // TODO Since we're doing the work anyway, store the result?
-                    let dist = map.pathfind(trip.path_req(map))?.total_dist(map);
+                    let dist = map.pathfind(trip.path_req(map))?.total_length();
                     // TODO This is failing all over the place, why?
                     assert!(dist <= trip.trip_dist);
                     let trip_time = (dist / trip.trip_dist) * trip.trip_time;
@@ -235,7 +235,7 @@ pub fn clip_trips(map: &Map, timer: &mut Timer) -> (Vec<Trip>, HashMap<BuildingI
             }
             (TripEndpt::Building(_), TripEndpt::Border(_, _)) => {
                 if false {
-                    let dist = map.pathfind(trip.path_req(map))?.total_dist(map);
+                    let dist = map.pathfind(trip.path_req(map))?.total_length();
                     assert!(dist <= trip.trip_dist);
                     trip.trip_time = (dist / trip.trip_dist) * trip.trip_time;
                     trip.trip_dist = dist;

sim/src/make/load.rs

@@ -27,6 +27,7 @@ impl SimFlags {
                 use_freeform_policy_everywhere: args.enabled("--freeform_policy"),
                 disable_block_the_box: args.enabled("--disable_block_the_box"),
                 record_stats: args.enabled("--record_stats"),
+                recalc_lanechanging: !args.enabled("--dont_recalc_lc"),
             },
         }
     }
@@ -40,13 +41,7 @@ impl SimFlags {
         SimFlags {
             load: abstutil::path_map(map),
             rng_seed: Some(42),
-            opts: SimOptions {
-                run_name: run_name.to_string(),
-                savestate_every: None,
-                use_freeform_policy_everywhere: false,
-                disable_block_the_box: false,
-                record_stats: false,
-            },
+            opts: SimOptions::new(run_name),
         }
     }
 

sim/src/mechanics/driving.rs

@@ -1,5 +1,5 @@
 use crate::mechanics::car::{Car, CarState};
-use crate::mechanics::queue::Queue;
+use crate::mechanics::Queue;
 use crate::{
     ActionAtEnd, AgentID, CarID, Command, CreateCar, DistanceInterval, DrawCarInput, Event,
     IntersectionSimState, ParkedCar, ParkingSimState, Scheduler, TimeInterval, TransitSimState,
@@ -33,14 +33,17 @@ pub struct DrivingSimState {
     )]
     queues: BTreeMap<Traversable, Queue>,
     events: Vec<Event>,
+
+    recalc_lanechanging: bool,
 }
 
 impl DrivingSimState {
-    pub fn new(map: &Map) -> DrivingSimState {
+    pub fn new(map: &Map, recalc_lanechanging: bool) -> DrivingSimState {
         let mut sim = DrivingSimState {
             cars: BTreeMap::new(),
             queues: BTreeMap::new(),
             events: Vec::new(),
+            recalc_lanechanging,
         };
 
         for l in map.all_lanes() {
@@ -245,6 +248,9 @@ impl DrivingSimState {
                 if queue.cars[0] == car.vehicle.id && queue.laggy_head.is_none() {
                     // Want to re-run, but no urgency about it happening immediately.
                     car.state = CarState::WaitingToAdvance;
+                    if self.recalc_lanechanging {
+                        car.router.opportunistically_lanechange(&self.queues, map);
+                    }
                     scheduler.push(now, Command::UpdateCar(car.vehicle.id));
                 }
             }
@@ -685,7 +691,12 @@ impl DrivingSimState {
                                 // The follower has been smoothly following while the laggy head gets out
                                 // of the way. So immediately promote them to WaitingToAdvance.
                                 follower.state = CarState::WaitingToAdvance;
-                                scheduler.push(now, Command::UpdateCar(*follower_id));
+                                if self.recalc_lanechanging {
+                                    follower
+                                        .router
+                                        .opportunistically_lanechange(&self.queues, map);
+                                }
+                                scheduler.push(now, Command::UpdateCar(follower.vehicle.id));
                             }
                         }
                         CarState::WaitingToAdvance => unreachable!(),

sim/src/mechanics/intersection.rs

@@ -1,5 +1,5 @@
 use crate::mechanics::car::Car;
-use crate::mechanics::queue::Queue;
+use crate::mechanics::Queue;
 use crate::{AgentID, Command, Scheduler, Speed};
 use abstutil::{deserialize_btreemap, serialize_btreemap};
 use derivative::Derivative;

sim/src/mechanics/mod.rs

@@ -8,4 +8,5 @@ mod walking;
 pub use self::driving::DrivingSimState;
 pub use self::intersection::IntersectionSimState;
 pub use self::parking::ParkingSimState;
+pub use self::queue::Queue;
 pub use self::walking::WalkingSimState;

sim/src/router.rs

@@ -1,3 +1,4 @@
+use crate::mechanics::Queue;
 use crate::{ParkingSimState, ParkingSpot, SidewalkSpot, Vehicle, VehicleType};
 use geom::Distance;
 use map_model::{
@@ -5,7 +6,7 @@ use map_model::{
     TurnID,
 };
 use serde_derive::{Deserialize, Serialize};
-use std::collections::{HashMap, VecDeque};
+use std::collections::{BTreeMap, HashMap, VecDeque};
 
 #[derive(Serialize, Deserialize, Clone, Debug, PartialEq)]
 pub struct Router {
@@ -237,6 +238,76 @@ impl Router {
             }
         }
     }
+
+    pub fn opportunistically_lanechange(
+        &mut self,
+        queues: &BTreeMap<Traversable, Queue>,
+        map: &Map,
+    ) {
+        let (current_turn, next_lane) = {
+            let steps = self.path.get_steps();
+            if steps.len() < 5 {
+                return;
+            }
+            match (steps[1], steps[4]) {
+                (PathStep::Turn(t), PathStep::Lane(l)) => (t, l),
+                _ => {
+                    return;
+                }
+            }
+        };
+
+        let orig_target_lane = current_turn.dst;
+        let parent = map.get_parent(orig_target_lane);
+        let next_parent = map.get_l(next_lane).src_i;
+
+        // Look for other candidate lanes. Must be the same lane type -- if there was a bus/bike
+        // lane originally and pathfinding already decided to use it, stick with that decision.
+        let orig_lt = map.get_l(orig_target_lane).lane_type;
+        let siblings = if parent.is_forwards(orig_target_lane) {
+            &parent.children_forwards
+        } else {
+            &parent.children_backwards
+        };
+
+        let (_, turn1, best_lane, turn2) = siblings
+            .iter()
+            .filter_map(|(l, lt)| {
+                let turn1 = TurnID {
+                    parent: current_turn.parent,
+                    src: current_turn.src,
+                    dst: *l,
+                };
+                if orig_lt == *lt && map.maybe_get_t(turn1).is_some() && map.is_turn_allowed(turn1)
+                {
+                    // Now make sure we can go from this lane to next_lane.
+                    let turn2 = TurnID {
+                        parent: next_parent,
+                        src: *l,
+                        dst: next_lane,
+                    };
+                    if map.maybe_get_t(turn2).is_some() && map.is_turn_allowed(turn2) {
+                        Some((queues[&Traversable::Lane(*l)].cars.len(), turn1, *l, turn2))
+                    } else {
+                        None
+                    }
+                } else {
+                    None
+                }
+            })
+            .min_by_key(|(len, _, _, _)| *len)
+            .unwrap();
+        // TODO Only switch if the target queue is some amount better; don't oscillate
+        // unnecessarily.
+        // TODO Better weight function... any slower vehicles in one?
+        if best_lane == orig_target_lane {
+            return;
+        }
+
+        self.path.modify_step(1, PathStep::Turn(turn1), map);
+        self.path.modify_step(2, PathStep::Lane(best_lane), map);
+        self.path.modify_step(3, PathStep::Turn(turn2), map);
+    }
 }
 
 // Unrealistically assumes the driver has knowledge of currently free parking spots, even if

sim/src/sim.rs

@@ -60,6 +60,7 @@ pub struct SimOptions {
     pub use_freeform_policy_everywhere: bool,
     pub disable_block_the_box: bool,
     pub record_stats: bool,
+    pub recalc_lanechanging: bool,
 }
 
 impl SimOptions {
@@ -70,6 +71,7 @@ impl SimOptions {
             use_freeform_policy_everywhere: false,
             disable_block_the_box: false,
             record_stats: false,
+            recalc_lanechanging: true,
         }
     }
 }
@@ -86,7 +88,7 @@ impl Sim {
             scheduler.push(d, Command::Savestate(d));
         }
         Sim {
-            driving: DrivingSimState::new(map),
+            driving: DrivingSimState::new(map, opts.recalc_lanechanging),
             parking: ParkingSimState::new(map),
             walking: WalkingSimState::new(),
             intersections: IntersectionSimState::new(