spawn cars anywhere along first lane. seems to work!

editor/src/plugins/sim/even_simpler_model.rs

@@ -133,6 +133,7 @@ fn populate_world(start: LaneID, map: &Map) -> new_des_model::World {
         }
     }
 
+    let mut counter = 0;
     let mut rng = XorShiftRng::from_seed([42; 16]);
     for source in sources {
         let len = map.get_l(source).length();
@@ -152,7 +153,7 @@ fn populate_world(start: LaneID, map: &Map) -> new_des_model::World {
             };
 
             world.spawn_car(
-                CarID::tmp_new(i, VehicleType::Car),
+                CarID::tmp_new(counter, VehicleType::Car),
                 max_speed,
                 path.clone(),
                 Duration::seconds(1.0) * (i as f64),
@@ -162,6 +163,7 @@ fn populate_world(start: LaneID, map: &Map) -> new_des_model::World {
                 )),
                 map,
             );
+            counter += 1;
         }
     }
 
@@ -171,8 +173,8 @@ fn populate_world(start: LaneID, map: &Map) -> new_des_model::World {
 fn densely_populate_world(map: &Map) -> new_des_model::World {
     let mut world = new_des_model::World::new(map);
     let mut rng = XorShiftRng::from_seed([42; 16]);
-
     let mut counter = 0;
+
     for l in map.all_lanes() {
         let len = l.length();
         if l.is_driving() && len >= new_des_model::VEHICLE_LENGTH {

editor/src/plugins/sim/new_des_model/car.rs

@@ -4,6 +4,7 @@ use map_model::{Map, Traversable};
 use sim::{CarID, DrawCarInput};
 use std::collections::VecDeque;
 
+#[derive(Debug)]
 pub struct Car {
     pub id: CarID,
     pub max_speed: Option<Speed>,
@@ -30,7 +31,7 @@ impl Car {
         self.last_steps = keep;
     }
 
-    pub fn get_draw_car(&self, front: Distance, map: &Map) -> Option<DrawCarInput> {
+    pub fn get_draw_car(&self, front: Distance, map: &Map) -> DrawCarInput {
         assert!(front >= Distance::ZERO);
         let body = if front >= VEHICLE_LENGTH {
             self.path[0]
@@ -47,8 +48,7 @@ impl Car {
             let mut i = 0;
             while leftover > Distance::ZERO {
                 if i == self.last_steps.len() {
-                    println!("{} spawned too close to short stuff", self.id);
+                    panic!("{} spawned too close to short stuff", self.id);
-                    return None;
                 }
                 let len = self.last_steps[i].length(map);
                 let start = (len - leftover).max(Distance::ZERO);
@@ -64,29 +64,31 @@ impl Car {
             PolyLine::new(result)
         };
 
-        Some(DrawCarInput {
+        DrawCarInput {
             id: self.id,
             waiting_for_turn: None,
             stopping_trace: None,
             state: match self.state {
                 // TODO Cars can be Queued behind a slow CrossingLane. Looks kind of weird.
                 CarState::Queued => sim::CarState::Stuck,
-                CarState::CrossingLane(_) => sim::CarState::Moving,
+                CarState::CrossingLane(_, _) => sim::CarState::Moving,
                 CarState::CrossingTurn(_) => sim::CarState::Moving,
             },
             vehicle_type: self.id.tmp_get_vehicle_type(),
             on: self.path[0],
             body,
-        })
+        }
     }
 }
 
+#[derive(Debug)]
 pub enum CarState {
-    CrossingLane(TimeInterval),
+    CrossingLane(TimeInterval, DistanceInterval),
     Queued,
     CrossingTurn(TimeInterval),
 }
 
+#[derive(Debug)]
 pub struct TimeInterval {
     pub start: Duration,
     pub end: Duration,
@@ -99,3 +101,23 @@ impl TimeInterval {
         x
     }
 }
+
+#[derive(Debug)]
+pub struct DistanceInterval {
+    pub start: Distance,
+    pub end: Distance,
+}
+
+impl DistanceInterval {
+    pub fn lerp(&self, x: f64) -> Distance {
+        assert!(x >= 0.0 && x <= 1.0);
+        self.start + x * (self.end - self.start)
+    }
+
+    pub fn upper_bound(&self, bound: Distance) -> DistanceInterval {
+        DistanceInterval {
+            start: self.start,
+            end: self.end.min(bound),
+        }
+    }
+}

editor/src/plugins/sim/new_des_model/intersection.rs

@@ -16,9 +16,7 @@ impl IntersectionController {
                 CarState::CrossingTurn(ref int) => int.percent(time),
                 _ => unreachable!(),
             };
-            if let Some(d) = car.get_draw_car(percent * t.geom.length(), map) {
+            return vec![car.get_draw_car(percent * t.geom.length(), map)];
-                return vec![d];
-            }
         }
         Vec::new()
     }

editor/src/plugins/sim/new_des_model/mod.rs

@@ -2,7 +2,7 @@ mod car;
 mod intersection;
 mod queue;
 
-pub use self::car::{Car, CarState, TimeInterval};
+pub use self::car::{Car, CarState, DistanceInterval, TimeInterval};
 use self::intersection::IntersectionController;
 use self::queue::Queue;
 use ezgui::{Color, GfxCtx};
@@ -69,7 +69,7 @@ impl World {
             let mut num_freeflow = 0;
             for car in &queue.cars {
                 match car.state {
-                    CarState::CrossingLane(_) => {
+                    CarState::CrossingLane(_, _) => {
                         num_freeflow += 1;
                     }
                     CarState::Queued => {
@@ -129,7 +129,7 @@ impl World {
                 queue
                     .get_car_positions(time)
                     .into_iter()
-                    .filter_map(|(car, dist)| car.get_draw_car(dist, map)),
+                    .map(|(car, dist)| car.get_draw_car(dist, map)),
             );
         }
         for i in self.intersections.values() {
@@ -149,7 +149,7 @@ impl World {
                 Some(q) => q
                     .get_car_positions(time)
                     .into_iter()
-                    .filter_map(|(car, dist)| car.get_draw_car(dist, map))
+                    .map(|(car, dist)| car.get_draw_car(dist, map))
                     .collect(),
                 None => Vec::new(),
             },
@@ -188,44 +188,11 @@ impl World {
     }
 
     pub fn step_if_needed(&mut self, time: Duration, map: &Map) {
-        // Spawn cars.
-        let mut retain_spawn = Vec::new();
-        for (id, max_speed, path, start_time, start_dist) in self.spawn_later.drain(..) {
-            let first_lane = path[0].as_lane();
-            if time >= start_time
-                && !self.queues[&first_lane].is_full()
-                && self.intersections[&map.get_l(first_lane).src_i]
-                    .accepted
-                    .as_ref()
-                    .map(|car| car.path[1].as_lane() != first_lane)
-                    .unwrap_or(true)
-            {
-                self.queues
-                    .get_mut(&first_lane)
-                    .unwrap()
-                    .cars
-                    .push_back(Car {
-                        id,
-                        max_speed,
-                        path: VecDeque::from(path.clone()),
-                        state: CarState::CrossingLane(TimeInterval {
-                            start: time,
-                            end: time
-                                + time_to_cross(Traversable::Lane(first_lane), map, max_speed),
-                        }),
-                        last_steps: VecDeque::new(),
-                    });
-            } else {
-                retain_spawn.push((id, max_speed, path, start_time, start_dist));
-            }
-        }
-        self.spawn_later = retain_spawn;
-
         // Promote CrossingLane to Queued.
         for queue in self.queues.values_mut() {
             for car in queue.cars.iter_mut() {
-                if let CarState::CrossingLane(ref interval) = car.state {
+                if let CarState::CrossingLane(ref time_int, _) = car.state {
-                    if time > interval.end {
+                    if time > time_int.end {
                         car.state = CarState::Queued;
                     }
                 }
@@ -281,7 +248,15 @@ impl World {
             car.trim_last_steps(map);
             car.state = CarState::CrossingTurn(TimeInterval {
                 start: time,
-                end: time + time_to_cross(Traversable::Turn(turn), map, car.max_speed),
+                end: time
+                    + time_to_cross(
+                        &DistanceInterval {
+                            start: Distance::ZERO,
+                            end: map.get_t(turn).geom.length(),
+                        },
+                        Traversable::Turn(turn).speed_limit(map),
+                        car.max_speed,
+                    ),
             });
             self.intersections.get_mut(&i).unwrap().accepted = Some(car);
         }
@@ -317,19 +292,85 @@ impl World {
                     i.id
                 );
             }
-            car.state = CarState::CrossingLane(TimeInterval {
+            let dist_int = DistanceInterval {
-                start: time,
+                start: Distance::ZERO,
-                end: end_time + time_to_cross(Traversable::Lane(lane), map, car.max_speed),
+                end: map.get_l(lane).length(),
-            });
+            };
+            let dt = time_to_cross(
+                &dist_int,
+                map.get_parent(lane).get_speed_limit(),
+                car.max_speed,
+            );
+            car.state = CarState::CrossingLane(
+                TimeInterval {
+                    start: time,
+                    end: time + dt,
+                },
+                dist_int,
+            );
             self.queues.get_mut(&lane).unwrap().cars.push_back(car);
         }
+
+        // Spawn cars at the end, so we can see the correct state of everything else at this time.
+        let mut retain_spawn = Vec::new();
+        for (id, max_speed, path, start_time, start_dist) in self.spawn_later.drain(..) {
+            let mut spawned = false;
+            let first_lane = path[0].as_lane();
+
+            if time >= start_time
+                && self.intersections[&map.get_l(first_lane).src_i]
+                    .accepted
+                    .as_ref()
+                    .map(|car| car.path[1].as_lane() != first_lane)
+                    .unwrap_or(true)
+            {
+                if let Some(idx) = self.queues[&first_lane].get_idx_to_insert_car(start_dist, time)
+                {
+                    let dist_int = DistanceInterval {
+                        start: start_dist,
+                        end: map.get_l(first_lane).length(),
+                    };
+                    let dt = time_to_cross(
+                        &dist_int,
+                        map.get_parent(first_lane).get_speed_limit(),
+                        max_speed,
+                    );
+                    self.queues.get_mut(&first_lane).unwrap().cars.insert(
+                        idx,
+                        Car {
+                            id,
+                            max_speed,
+                            path: VecDeque::from(path.clone()),
+                            state: CarState::CrossingLane(
+                                TimeInterval {
+                                    start: time,
+                                    end: time + dt,
+                                },
+                                dist_int,
+                            ),
+                            last_steps: VecDeque::new(),
+                        },
+                    );
+                    spawned = true;
+                    //println!("{} spawned at {}", id, time);
+                }
+            }
+            if !spawned {
+                retain_spawn.push((id, max_speed, path, start_time, start_dist));
+            }
+        }
+        self.spawn_later = retain_spawn;
     }
 }
 
-fn time_to_cross(on: Traversable, map: &Map, max_speed: Option<Speed>) -> Duration {
+fn time_to_cross(
-    let mut speed = on.speed_limit(map);
+    dist_int: &DistanceInterval,
+    speed_limit: Speed,
+    max_speed: Option<Speed>,
+) -> Duration {
+    let mut speed = speed_limit;
     if let Some(s) = max_speed {
         speed = speed.min(s);
     }
-    on.length(map) / speed
+    (dist_int.end - dist_int.start) / speed
 }

editor/src/plugins/sim/new_des_model/queue.rs

@@ -22,6 +22,7 @@ impl Queue {
 
     // May not return all of the cars -- some might be temporarily unable to actually enter the end
     // of the road.
+    // Farthest along (greatest distance) is first.
     pub fn get_car_positions(&self, time: Duration) -> Vec<(&Car, Distance)> {
         if self.cars.is_empty() {
             return Vec::new();
@@ -35,23 +36,91 @@ impl Queue {
                 None => self.lane_len,
             };
 
+            // There's spillover and a car shouldn't have been able to enter yet, but it's a
+            // temporary condition -- there's enough rest capacity.
+            if bound < Distance::ZERO {
+                println!(
+                    "Queue temporarily backed up on {} at {} -- can't draw {}",
+                    self.id, time, car.id
+                );
+                return validate_positions(result, time, self.id);
+            }
+
             let front = match car.state {
                 CarState::Queued => bound,
-                CarState::CrossingLane(ref i) => i.percent(time) * bound,
+                CarState::CrossingLane(ref time_int, ref dist_int) => {
+                    dist_int.upper_bound(bound).lerp(time_int.percent(time))
+                }
                 CarState::CrossingTurn(_) => unreachable!(),
             };
 
-            // There's backfill and a car shouldn't have been able to enter yet, but it's a
-            // temporary condition -- there's enough rest capacity.
-            if front < Distance::ZERO {
-                println!(
-                    "Queue temporarily backed up on {} -- can't draw {}",
-                    self.id, car.id
-                );
-                return result;
-            }
             result.push((car, front));
         }
-        result
+        validate_positions(result, time, self.id)
+    }
+
+    pub fn get_idx_to_insert_car(&self, start_dist: Distance, time: Duration) -> Option<usize> {
+        if self.is_full() {
+            return None;
+        }
+        if self.is_empty() {
+            return Some(0);
+        }
+
+        let dists = self.get_car_positions(time);
+        // TODO Binary search
+        let idx = match dists.iter().position(|(_, dist)| start_dist <= *dist) {
+            Some(i) => i + 1,
+            None => 0,
+        };
+
+        // Are we too close to the leader?
+        if idx != 0 && dists[idx - 1].1 - VEHICLE_LENGTH - FOLLOWING_DISTANCE < start_dist {
+            return None;
+        }
+        // Or the follower?
+        if idx != dists.len() && start_dist - VEHICLE_LENGTH - FOLLOWING_DISTANCE < dists[idx].1 {
+            return None;
+        }
+
+        // If there's temporary spillover, but we want to spawn in front of the mess, that's fine
+        if dists.len() < self.cars.len() && idx != 0 {
+            return None;
+        }
+
+        Some(idx)
     }
 }
+
+fn validate_positions(
+    cars: Vec<(&Car, Distance)>,
+    time: Duration,
+    id: LaneID,
+) -> Vec<(&Car, Distance)> {
+    for pair in cars.windows(2) {
+        if pair[0].1 - VEHICLE_LENGTH - FOLLOWING_DISTANCE < pair[1].1 {
+            println!("\nOn {} at {}...", id, time);
+            for (car, dist) in &cars {
+                println!("- {} @ {}", car.id, dist);
+                match car.state {
+                    CarState::CrossingLane(ref time_int, ref dist_int) => {
+                        println!(
+                            "  Going {} .. {} during {} .. {}",
+                            dist_int.start, dist_int.end, time_int.start, time_int.end
+                        );
+                    }
+                    CarState::Queued => {
+                        println!("  Queued currently");
+                    }
+                    CarState::CrossingTurn(_) => unreachable!(),
+                }
+            }
+            println!();
+            panic!(
+                "get_car_positions wound up with bad positioning: {} then {}\n{:?}",
+                pair[0].1, pair[1].1, cars
+            );
+        }
+    }
+    cars
+}