diff --git a/editor/src/ui.rs b/editor/src/ui.rs index 61353f153b..099da44ff6 100644 --- a/editor/src/ui.rs +++ b/editor/src/ui.rs @@ -512,6 +512,10 @@ impl gui::GUI for UI { self.sim_ctrl.sim.toggle_debug(id); return gui::EventLoopMode::InputOnly; } + if input.key_pressed(Key::A, "start this parked car") { + self.sim_ctrl.sim.start_parked_car(&self.map, id); + return gui::EventLoopMode::InputOnly; + } } SelectionState::SelectedLane(id, _) => { if input.key_pressed(Key::F, "start floodfilling from this lane") { @@ -519,9 +523,11 @@ impl gui::GUI for UI { return gui::EventLoopMode::InputOnly; } - if input.key_pressed(Key::A, "start something on this lane") { - self.sim_ctrl.sim.start_agent(&self.map, id); - return gui::EventLoopMode::InputOnly; + if self.map.get_l(id).lane_type == map_model::LaneType::Sidewalk { + if input.key_pressed(Key::A, "spawn a pedestrian here") { + self.sim_ctrl.sim.spawn_pedestrian(&self.map, id); + return gui::EventLoopMode::InputOnly; + } } } SelectionState::SelectedIntersection(id) => { diff --git a/map_model/src/map.rs b/map_model/src/map.rs index 82e2678730..7784637b58 100644 --- a/map_model/src/map.rs +++ b/map_model/src/map.rs @@ -308,9 +308,9 @@ impl Map { .collect() } - pub fn get_lane_and_parent(&self, id: LaneID) -> (&Lane, &Road) { + pub fn get_parent(&self, id: LaneID) -> &Road { let l = self.get_l(id); - (l, self.get_r(l.parent)) + self.get_r(l.parent) } // TODO can we return a borrow? diff --git a/sim/src/driving.rs b/sim/src/driving.rs index 8cc3b424e7..d63f64add7 100644 --- a/sim/src/driving.rs +++ b/sim/src/driving.rs @@ -5,7 +5,7 @@ use draw_car::DrawCar; use intersections::{AgentInfo, IntersectionSimState, Request}; use kinematics; use kinematics::Vehicle; -use map_model::{LaneID, LaneType, Map, TurnID}; +use map_model::{LaneID, Map, TurnID}; use models::{choose_turn, FOLLOWING_DISTANCE}; use multimap::MultiMap; use ordered_float::NotNaN; @@ -296,6 +296,14 @@ impl SimQueue { .find(|id| sim.cars[id].dist_along > dist) .map(|id| *id) } + + fn insert_at(&mut self, car: CarID, dist_along: Distance, sim: &DrivingSimState) { + if let Some(idx) = self.cars_queue.iter().position(|id| sim.cars[id].dist_along < dist_along) { + self.cars_queue.insert(idx, car); + } else { + self.cars_queue.push(car); + } + } } // This manages only actively driving cars @@ -488,45 +496,37 @@ impl DrivingSimState { Ok(()) } - // TODO cars basically start in the intersection, with their front bumper right at the - // beginning of the lane. later, we want cars starting at arbitrary points in the middle of the - // lane (from a building), so just ignore this problem for now. // True if we spawned one pub fn start_car_on_lane( &mut self, _time: Tick, car: CarID, + dist_along: Distance, mut path: VecDeque, - _map: &Map, + map: &Map, ) -> bool { let start = path.pop_front().unwrap(); + // If not, we have a parking lane much longer than a driving lane... + assert!(dist_along <= map.get_l(start).length()); + + // TODO verify it's safe to appear here at dist_along and not cause a crash self.cars.insert( car, Car { id: car, path, - dist_along: 0.0 * si::M, + dist_along: dist_along, speed: 0.0 * si::MPS, on: On::Lane(start), waiting_for: None, debug: false, }, ); - self.lanes[start.0].cars_queue.push(car); + self.lanes[start.0].insert_at(car, dist_along, self); true } - pub fn get_empty_lanes(&self, map: &Map) -> Vec { - let mut lanes: Vec = Vec::new(); - for (idx, queue) in self.lanes.iter().enumerate() { - if map.get_l(LaneID(idx)).lane_type == LaneType::Driving && queue.is_empty() { - lanes.push(queue.id.as_lane()); - } - } - lanes - } - pub fn get_draw_car(&self, id: CarID, _time: Tick, map: &Map) -> Option { let c = self.cars.get(&id)?; let (pos, angle) = c.on.dist_along(c.dist_along, map); diff --git a/sim/src/lib.rs b/sim/src/lib.rs index 370b4a637e..d6ebb11cc3 100644 --- a/sim/src/lib.rs +++ b/sim/src/lib.rs @@ -155,8 +155,8 @@ impl On { fn speed_limit(&self, map: &Map) -> Speed { match self { - &On::Lane(id) => map.get_lane_and_parent(id).1.get_speed_limit(), - &On::Turn(id) => map.get_lane_and_parent(id.dst).1.get_speed_limit(), + &On::Lane(id) => map.get_parent(id).get_speed_limit(), + &On::Turn(id) => map.get_parent(id.dst).get_speed_limit(), } } } diff --git a/sim/src/parametric_driving.rs b/sim/src/parametric_driving.rs index a1b8074b21..f879164965 100644 --- a/sim/src/parametric_driving.rs +++ b/sim/src/parametric_driving.rs @@ -10,7 +10,7 @@ use draw_car::DrawCar; use geom::{Angle, Pt2D}; use intersections::{AgentInfo, IntersectionSimState, Request}; use kinematics::Vehicle; -use map_model::{LaneID, LaneType, Map, TurnID}; +use map_model::{LaneID, Map, TurnID}; use models::{choose_turn, Action, FOLLOWING_DISTANCE}; use multimap::MultiMap; use std::collections::{BTreeMap, HashSet, VecDeque}; @@ -459,6 +459,7 @@ impl DrivingSimState { &mut self, time: Tick, car: CarID, + _dist_along: Distance, mut path: VecDeque, map: &Map, ) -> bool { @@ -485,16 +486,6 @@ impl DrivingSimState { true } - pub fn get_empty_lanes(&self, map: &Map) -> Vec { - let mut lanes: Vec = Vec::new(); - for (idx, queue) in self.lanes.iter().enumerate() { - if map.get_l(LaneID(idx)).lane_type == LaneType::Driving && queue.is_empty() { - lanes.push(queue.id.as_lane()); - } - } - lanes - } - pub fn get_draw_car(&self, id: CarID, time: Tick, map: &Map) -> Option { let all = match self.cars.get(&id)?.on { On::Lane(l) => self.get_draw_cars_on_lane(l, time, map), diff --git a/sim/src/parking.rs b/sim/src/parking.rs index 3b713f53ae..6b18846168 100644 --- a/sim/src/parking.rs +++ b/sim/src/parking.rs @@ -5,7 +5,7 @@ use map_model; use map_model::{Lane, LaneID, LaneType, Map}; use rand::Rng; use std::iter; -use CarID; +use {Distance, CarID}; #[derive(Serialize, Deserialize, PartialEq, Eq)] pub struct ParkingSimState { @@ -28,7 +28,7 @@ impl ParkingSimState { pub fn edit_remove_lane(&mut self, id: LaneID) { assert!(self.lanes[id.0].is_empty()); self.lanes[id.0] = ParkingLane { - l: id, + id: id, spots: Vec::new(), }; } @@ -72,12 +72,8 @@ impl ParkingSimState { ); } - pub fn get_last_parked_car(&self, id: LaneID) -> Option { - self.lanes[id.0].get_last_parked_car() - } - - pub fn remove_last_parked_car(&mut self, id: LaneID, car: CarID) { - self.lanes[id.0].remove_last_parked_car(car); + pub fn remove_parked_car(&mut self, id: LaneID, car: CarID) { + self.lanes[id.0].remove_parked_car(car); self.total_count -= 1; } @@ -94,11 +90,41 @@ impl ParkingSimState { } None } + + pub fn lane_of_car(&self, id: CarID) -> Option { + // TODO this is so horrendously slow :D + for l in &self.lanes { + if l.spots.contains(&Some(id)) { + return Some(l.id); + } + } + None + } + + // Of the front of the car + pub fn get_dist_along_lane(&self, c: CarID, l: LaneID) -> Distance { + let idx = self.lanes[l.0].spots.iter().position(|x| *x == Some(c)).unwrap(); + // TODO some overlap + let spot_start = map_model::PARKING_SPOT_LENGTH * (1.0 + idx as f64); + spot_start - (map_model::PARKING_SPOT_LENGTH - draw_car::CAR_LENGTH) / 2.0 + } + + pub fn get_all_cars(&self) -> Vec<(CarID, LaneID)> { + let mut result = Vec::new(); + for l in &self.lanes { + for maybe_car in &l.spots { + if let Some(car) = maybe_car { + result.push((*car, l.id)); + } + } + } + result + } } #[derive(Serialize, Deserialize, PartialEq, Eq)] struct ParkingLane { - l: LaneID, + id: LaneID, spots: Vec>, } @@ -106,35 +132,24 @@ impl ParkingLane { fn new(l: &Lane) -> ParkingLane { if l.lane_type != LaneType::Parking { return ParkingLane { - l: l.id, + id: l.id, spots: Vec::new(), }; } ParkingLane { - l: l.id, + id: l.id, spots: iter::repeat(None).take(l.number_parking_spots()).collect(), } } - fn get_last_parked_car(&self) -> Option { - self.spots - .iter() - .rfind(|&&x| x.is_some()) - .map(|l| l.unwrap()) - } - - fn remove_last_parked_car(&mut self, car: CarID) { - let idx = self.spots - .iter() - .rposition(|&x| x.is_some()) - .expect("No parked cars at all now"); - assert_eq!(self.spots[idx], Some(car)); + fn remove_parked_car(&mut self, car: CarID) { + let idx = self.spots.iter().position(|x| *x == Some(car)).unwrap(); self.spots[idx] = None; } fn get_draw_cars(&self, map: &Map) -> Vec { - let l = map.get_l(self.l); + let l = map.get_l(self.id); // TODO this is slow to do constantly! can we precompute for each spot or something like // that? self.spots @@ -152,7 +167,7 @@ impl ParkingLane { .collect() } - pub fn is_empty(&self) -> bool { - !self.get_last_parked_car().is_some() + fn is_empty(&self) -> bool { + !self.spots.iter().find(|&&x| x.is_some()).is_some() } } diff --git a/sim/src/sim.rs b/sim/src/sim.rs index 577a67719e..0a2359307b 100644 --- a/sim/src/sim.rs +++ b/sim/src/sim.rs @@ -15,7 +15,7 @@ use std::collections::{HashMap, HashSet, VecDeque}; use std::f64; use std::time::{Duration, Instant}; use walking::WalkingSimState; -use {CarID, CarState, InvariantViolated, PedestrianID, Tick, TIMESTEP}; +use {CarID, CarState, InvariantViolated, PedestrianID, Tick, TIMESTEP, Distance}; #[derive(Serialize, Deserialize, Derivative, PartialEq, Eq)] enum DrivingModel { @@ -90,10 +90,10 @@ impl DrivingModel { &mut self, time: Tick, car: CarID, + dist_along: Distance, path: VecDeque, map: &Map ) -> bool); - delegate!(fn get_empty_lanes(&self, map: &Map) -> Vec); delegate!(fn get_draw_car(&self, id: CarID, time: Tick, map: &Map) -> Option); delegate!(fn get_draw_cars_on_lane(&self, lane: LaneID, time: Tick, map: &Map) -> Vec); delegate!(fn get_draw_cars_on_turn(&self, turn: TurnID, time: Tick, map: &Map) -> Vec); @@ -179,35 +179,52 @@ impl Sim { pub fn start_many_parked_cars(&mut self, map: &Map, num_cars: usize) { use rayon::prelude::*; - let mut driving_lanes = self.driving_state.get_empty_lanes(map); - // Don't ruin determinism for silly reasons. :) - if !driving_lanes.is_empty() { - self.rng.shuffle(&mut driving_lanes); + let mut cars_and_starts: Vec<(CarID, LaneID)> = self.parking_state + .get_all_cars() + .into_iter() + .filter_map(|(car, parking_lane)| { + map.get_parent(parking_lane) + .find_driving_lane(parking_lane) + .and_then(|driving_lane| Some((car, driving_lane))) + }) + .collect(); + if cars_and_starts.is_empty() { + return; } + self.rng.shuffle(&mut cars_and_starts); - let mut requested_paths: Vec<(LaneID, LaneID)> = Vec::new(); - for i in 0..num_cars.min(driving_lanes.len()) { - let start = driving_lanes[i]; + let driving_lanes: Vec = map.all_lanes() + .iter() + .filter_map(|l| { + if l.lane_type == LaneType::Driving { + Some(l.id) + } else { + None + } + }) + .collect(); + let mut requested_paths: Vec<(CarID, LaneID, LaneID)> = Vec::new(); + for i in 0..num_cars.min(cars_and_starts.len()) { + let (car, start) = cars_and_starts[i]; let goal = choose_different(&mut self.rng, &driving_lanes, start); - requested_paths.push((start, goal)); + requested_paths.push((car, start, goal)); } println!("Calculating {} paths for cars", requested_paths.len()); let timer = Instant::now(); - let paths: Vec>> = requested_paths + let paths: Vec<(CarID, Option>)> = requested_paths .par_iter() - .map(|(start, goal)| map_model::pathfind(map, *start, *goal)) + .map(|(car, start, goal)| (*car, map_model::pathfind(map, *start, *goal))) .collect(); let mut actual = 0; - for path in paths.into_iter() { + for (car, path) in paths.into_iter() { if let Some(steps) = path { - if self.start_parked_car(map, steps) { + if self.start_parked_car_with_path(car, map, steps) { actual += 1; } } else { - // zip with request to have start/goal? - //println!("Failed to pathfind for a pedestrian"); + println!("Failed to pathfind for {}", car); }; } @@ -219,62 +236,49 @@ impl Sim { println!("Started {} parked cars of requested {}", actual, num_cars); } - fn start_parked_car(&mut self, map: &Map, steps: Vec) -> bool { + fn start_parked_car_with_path(&mut self, car: CarID, map: &Map, steps: Vec) -> bool { let driving_lane = steps[0]; - if let Some(parking_lane) = map.get_lane_and_parent(driving_lane) - .1 + let parking_lane = map.get_parent(driving_lane) .find_parking_lane(driving_lane) + .unwrap(); + let dist_along = self.parking_state.get_dist_along_lane(car, parking_lane); + + if self.driving_state + .start_car_on_lane(self.time, car, dist_along, VecDeque::from(steps), map) { - if let Some(car) = self.parking_state.get_last_parked_car(parking_lane) { - if self.driving_state - .start_car_on_lane(self.time, car, VecDeque::from(steps), map) - { - self.parking_state.remove_last_parked_car(parking_lane, car); - return true; - } - } else { - println!("No parked cars on {}", parking_lane); - } + self.parking_state.remove_parked_car(parking_lane, car); + true } else { - println!("{} has no parking lane", driving_lane); + false } - false } - // TODO make the UI do some of this - pub fn start_agent(&mut self, map: &Map, id: LaneID) -> bool { - // TODO maybe a way to grab both? - let (lane, road) = map.get_lane_and_parent(id); - let driving_lane = match lane.lane_type { - LaneType::Sidewalk => { - if let Some(path) = pick_goal_and_find_path(&mut self.rng, map, id) { - println!("Spawned a pedestrian at {}", id); - self.walking_state - .seed_pedestrian(map, VecDeque::from(path)); - return true; - } else { - return false; - } - } - LaneType::Driving => id, - LaneType::Parking => { - if let Some(driving) = road.find_driving_lane(id) { - driving - } else { - println!("{} has no driving lane", id); - return false; - } - } - LaneType::Biking => { - println!("TODO implement bikes"); - return false; - } - }; + pub fn start_parked_car(&mut self, map: &Map, car: CarID) -> bool { + let parking_lane = self.parking_state + .lane_of_car(car) + .expect("Car isn't parked"); + let road = map.get_parent(parking_lane); + let driving_lane = road.find_driving_lane(parking_lane) + .expect("Parking lane has no driving lane"); if let Some(path) = pick_goal_and_find_path(&mut self.rng, map, driving_lane) { - return self.start_parked_car(map, path); + self.start_parked_car_with_path(car, map, path) + } else { + false + } + } + + pub fn spawn_pedestrian(&mut self, map: &Map, sidewalk: LaneID) -> bool { + assert!(map.get_l(sidewalk).lane_type == LaneType::Sidewalk); + + if let Some(path) = pick_goal_and_find_path(&mut self.rng, map, sidewalk) { + self.walking_state + .seed_pedestrian(map, VecDeque::from(path)); + println!("Spawned a pedestrian at {}", sidewalk); + true + } else { + false } - false } pub fn seed_pedestrians(&mut self, map: &Map, num: usize) {