From fd7b20b01c11677739f7527f4e78efbbc2f76bf3 Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Wed, 20 Feb 2019 12:30:33 -0800 Subject: [PATCH] spawn cars anywhere along first lane. seems to work! --- editor/src/plugins/sim/even_simpler_model.rs | 6 +- editor/src/plugins/sim/new_des_model/car.rs | 36 ++++- .../plugins/sim/new_des_model/intersection.rs | 4 +- editor/src/plugins/sim/new_des_model/mod.rs | 135 ++++++++++++------ editor/src/plugins/sim/new_des_model/queue.rs | 91 ++++++++++-- 5 files changed, 202 insertions(+), 70 deletions(-) diff --git a/editor/src/plugins/sim/even_simpler_model.rs b/editor/src/plugins/sim/even_simpler_model.rs index 34ea3db8a3..77cc45d91f 100644 --- a/editor/src/plugins/sim/even_simpler_model.rs +++ b/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 { diff --git a/editor/src/plugins/sim/new_des_model/car.rs b/editor/src/plugins/sim/new_des_model/car.rs index c76181c646..d21e5cc6cd 100644 --- a/editor/src/plugins/sim/new_des_model/car.rs +++ b/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, @@ -30,7 +31,7 @@ impl Car { self.last_steps = keep; } - pub fn get_draw_car(&self, front: Distance, map: &Map) -> Option { + 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); - return None; + panic!("{} spawned too close to short stuff", self.id); } 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), + } + } +} diff --git a/editor/src/plugins/sim/new_des_model/intersection.rs b/editor/src/plugins/sim/new_des_model/intersection.rs index c0fd7eaab3..23e9bc4095 100644 --- a/editor/src/plugins/sim/new_des_model/intersection.rs +++ b/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![d]; - } + return vec![car.get_draw_car(percent * t.geom.length(), map)]; } Vec::new() } diff --git a/editor/src/plugins/sim/new_des_model/mod.rs b/editor/src/plugins/sim/new_des_model/mod.rs index 6d72b7391b..a440480076 100644 --- a/editor/src/plugins/sim/new_des_model/mod.rs +++ b/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 time > interval.end { + if let CarState::CrossingLane(ref time_int, _) = car.state { + 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 { - start: time, - end: end_time + time_to_cross(Traversable::Lane(lane), map, car.max_speed), - }); + let dist_int = DistanceInterval { + start: Distance::ZERO, + 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) -> Duration { - let mut speed = on.speed_limit(map); +fn time_to_cross( + dist_int: &DistanceInterval, + speed_limit: Speed, + max_speed: Option, +) -> 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 } diff --git a/editor/src/plugins/sim/new_des_model/queue.rs b/editor/src/plugins/sim/new_des_model/queue.rs index ed8230032f..cae1592375 100644 --- a/editor/src/plugins/sim/new_des_model/queue.rs +++ b/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 { + 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 +}