diff --git a/sim/src/lib.rs b/sim/src/lib.rs index 568836171a..f1bd6701b9 100644 --- a/sim/src/lib.rs +++ b/sim/src/lib.rs @@ -86,8 +86,8 @@ impl Tick { (self.0 as f64) * TIMESTEP } - pub fn increment(&mut self) { - self.0 += 1; + pub fn next(self) -> Tick { + Tick(self.0 + 1) } // TODO er, little weird diff --git a/sim/src/sim.rs b/sim/src/sim.rs index 25372ec3b0..1d901a8567 100644 --- a/sim/src/sim.rs +++ b/sim/src/sim.rs @@ -278,68 +278,25 @@ impl Sim { } } - pub fn spawn_pedestrian(&mut self, map: &Map, sidewalk: LaneID) -> bool { - assert!(map.get_l(sidewalk).is_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 - } + pub fn spawn_pedestrian(&mut self, map: &Map, sidewalk: LaneID) { + self.spawner + .spawn_pedestrian(self.time.next(), map, sidewalk, &mut self.rng); } pub fn seed_pedestrians(&mut self, map: &Map, num: usize) { - use rayon::prelude::*; - - let mut sidewalks: Vec = Vec::new(); - for l in map.all_lanes() { - if l.is_sidewalk() { - sidewalks.push(l.id); - } - } - - let mut requested_paths: Vec<(LaneID, LaneID)> = Vec::new(); - for _i in 0..num { - let start = *self.rng.choose(&sidewalks).unwrap(); - let goal = choose_different(&mut self.rng, &sidewalks, start); - requested_paths.push((start, goal)); - } - - println!("Calculating {} paths for pedestrians", num); - // TODO better timer macro - let timer = Instant::now(); - let paths: Vec>> = requested_paths - .par_iter() - .map(|(start, goal)| map_model::pathfind(map, *start, *goal)) - .collect(); - - let mut actual = 0; - for path in paths.into_iter() { - if let Some(steps) = path { - self.walking_state - .seed_pedestrian(map, VecDeque::from(steps)); - actual += 1; - } else { - // zip with request to have start/goal? - //println!("Failed to pathfind for a pedestrian"); - }; - } - - println!( - "Calculating {} pedestrian paths took {:?}", - num, - timer.elapsed() - ); - println!("Spawned {} pedestrians of requested {}", actual, num); + self.spawner + .spawn_many_pedestrians(self.time.next(), map, num, &mut self.rng); } pub fn step(&mut self, map: &Map, control_map: &ControlMap) { - self.time.increment(); + self.time = self.time.next(); - self.spawner.step(self.time, &mut self.parking_state); + self.spawner.step( + self.time, + map, + &mut self.parking_state, + &mut self.walking_state, + ); match self.driving_state.step( self.time, diff --git a/sim/src/spawn.rs b/sim/src/spawn.rs index f945fc90d0..0000505e8f 100644 --- a/sim/src/spawn.rs +++ b/sim/src/spawn.rs @@ -1,7 +1,11 @@ -use map_model::LaneID; +use map_model; +use map_model::{LaneID, Map}; use parking::ParkingSimState; use rand::Rng; use sim::CarParking; +use std::collections::VecDeque; +use std::time::Instant; +use walking::WalkingSimState; use {CarID, PedestrianID, Tick}; // TODO move the stuff in sim that does RNG stuff, picks goals, etc to here. make the UI commands @@ -15,6 +19,15 @@ enum Command { SpawnPedestrian(Tick, PedestrianID, LaneID, LaneID), } +impl Command { + fn get_time(&self) -> Tick { + match self { + Command::StartParkedCar(time, _, _) => *time, + Command::SpawnPedestrian(time, _, _, _) => *time, + } + } +} + // This must get the car/ped IDs correct. #[derive(Serialize, Deserialize, PartialEq, Eq)] pub struct Spawner { @@ -23,24 +36,81 @@ pub struct Spawner { spawn_parked_cars: Vec, // Ordered by time - commands: Vec, + commands: VecDeque, car_id_counter: usize, + ped_id_counter: usize, } impl Spawner { pub fn empty() -> Spawner { Spawner { spawn_parked_cars: Vec::new(), - commands: Vec::new(), + commands: VecDeque::new(), car_id_counter: 0, + ped_id_counter: 0, } } - pub fn step(&mut self, _time: Tick, parking_sim: &mut ParkingSimState) { + pub fn step( + &mut self, + now: Tick, + map: &Map, + parking_sim: &mut ParkingSimState, + walking_sim: &mut WalkingSimState, + ) { for p in self.spawn_parked_cars.drain(0..) { parking_sim.add_parked_car(p); } + + let mut spawn_peds: Vec = Vec::new(); + let mut requested_paths: Vec<(LaneID, LaneID)> = Vec::new(); + loop { + let pop = if let Some(cmd) = self.commands.front() { + match cmd { + Command::StartParkedCar(time, car, goal) => { + println!("TODO"); + false + } + Command::SpawnPedestrian(time, ped, start, goal) => { + if now == *time { + spawn_peds.push(*ped); + requested_paths.push((*start, *goal)); + true + } else { + false + } + } + } + } else { + false + }; + if pop { + self.commands.pop_front(); + } else { + break; + } + } + + if requested_paths.is_empty() { + return; + } + + let paths = calculate_paths(&requested_paths, map); + let mut spawned_peds = 0; + for (ped, (req, maybe_path)) in spawn_peds.iter().zip(requested_paths.iter().zip(paths)) { + if let Some(path) = maybe_path { + walking_sim.seed_pedestrian(*ped, map, VecDeque::from(path)); + spawned_peds += 1; + } else { + println!("Couldn't find path from {} to {} for {}", req.0, req.1, ped); + } + } + println!( + "Spawned {} pedestrians of requested {}", + spawned_peds, + spawn_peds.len() + ); } // TODO the mut is temporary @@ -72,4 +142,85 @@ impl Spawner { new_cars, total_capacity ); } + + pub fn spawn_pedestrian( + &mut self, + at: Tick, + map: &Map, + sidewalk: LaneID, + rng: &mut R, + ) { + if let Some(cmd) = self.commands.back() { + assert!(at >= cmd.get_time()); + } + assert!(map.get_l(sidewalk).is_sidewalk()); + + let goal = pick_goal(rng, map, sidewalk); + self.commands.push_back(Command::SpawnPedestrian( + at, + PedestrianID(self.ped_id_counter), + sidewalk, + goal, + )); + self.ped_id_counter += 1; + println!("Spawned a pedestrian at {}", sidewalk); + } + + pub fn spawn_many_pedestrians( + &mut self, + at: Tick, + map: &Map, + num: usize, + rng: &mut R, + ) { + if let Some(cmd) = self.commands.back() { + assert!(at >= cmd.get_time()); + } + + let mut sidewalks: Vec = Vec::new(); + for l in map.all_lanes() { + if l.is_sidewalk() { + sidewalks.push(l.id); + } + } + + for _i in 0..num { + let start = *rng.choose(&sidewalks).unwrap(); + self.spawn_pedestrian(at, map, start, rng); + } + } +} + +fn pick_goal(rng: &mut R, map: &Map, start: LaneID) -> LaneID { + let lane_type = map.get_l(start).lane_type; + let candidate_goals: Vec = map.all_lanes() + .iter() + .filter_map(|l| { + if l.lane_type != lane_type || l.id == start { + None + } else { + Some(l.id) + } + }) + .collect(); + *rng.choose(&candidate_goals).unwrap() +} + +fn calculate_paths(requested_paths: &Vec<(LaneID, LaneID)>, map: &Map) -> Vec>> { + use rayon::prelude::*; + + println!("Calculating {} paths", requested_paths.len()); + // TODO better timer macro + let timer = Instant::now(); + let paths: Vec>> = requested_paths + .par_iter() + .map(|(start, goal)| map_model::pathfind(map, *start, *goal)) + .collect(); + + println!( + "Calculating {} paths took {:?}", + paths.len(), + timer.elapsed() + ); + paths } diff --git a/sim/src/walking.rs b/sim/src/walking.rs index e76e9d64e9..1e20d13b79 100644 --- a/sim/src/walking.rs +++ b/sim/src/walking.rs @@ -147,8 +147,6 @@ pub struct WalkingSimState { #[serde(serialize_with = "serialize_multimap")] #[serde(deserialize_with = "deserialize_multimap")] peds_per_turn: MultiMap, - - id_counter: usize, } impl WalkingSimState { @@ -157,7 +155,6 @@ impl WalkingSimState { peds: BTreeMap::new(), peds_per_sidewalk: MultiMap::new(), peds_per_turn: MultiMap::new(), - id_counter: 0, } } @@ -272,10 +269,7 @@ impl WalkingSimState { result } - pub fn seed_pedestrian(&mut self, map: &Map, mut path: VecDeque) { - let id = PedestrianID(self.id_counter); - self.id_counter += 1; - + pub fn seed_pedestrian(&mut self, id: PedestrianID, map: &Map, mut path: VecDeque) { let start = path.pop_front().unwrap(); let contraflow = is_contraflow(map, start, path[0]); self.peds.insert(