From 1720e518286ef3a53a533f4fb568de1094e657bc Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Tue, 14 Aug 2018 19:45:45 -0700 Subject: [PATCH] WIP start a specific parked car, not the last one in the lane. broken, because multiple cars jump to the beginning of the driving lane. need to make them start in the right position (and then go there slowly) --- editor/src/ui.rs | 12 +++- map_model/src/map.rs | 4 +- sim/src/driving.rs | 34 ++++----- sim/src/lib.rs | 4 +- sim/src/parametric_driving.rs | 13 +--- sim/src/parking.rs | 69 +++++++++++------- sim/src/sim.rs | 128 ++++++++++++++++++---------------- 7 files changed, 140 insertions(+), 124 deletions(-) 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) {