diff --git a/editor/src/plugins/sim/new_des_model/intersection.rs b/editor/src/plugins/sim/new_des_model/intersection.rs index 3ce7450ab7..604729c59a 100644 --- a/editor/src/plugins/sim/new_des_model/intersection.rs +++ b/editor/src/plugins/sim/new_des_model/intersection.rs @@ -1,15 +1,22 @@ use crate::plugins::sim::new_des_model::Queue; use geom::Duration; -use map_model::{IntersectionID, LaneID, Traversable, TurnID}; +use map_model::{IntersectionID, LaneID, Map, Traversable, TurnID}; use sim::CarID; -use std::collections::BTreeMap; +use std::collections::{BTreeMap, HashSet}; pub struct IntersectionController { - pub id: IntersectionID, - pub accepted: Option<(CarID, TurnID)>, + _id: IntersectionID, + accepted: HashSet<(CarID, TurnID)>, } impl IntersectionController { + pub fn new(id: IntersectionID) -> IntersectionController { + IntersectionController { + _id: id, + accepted: HashSet::new(), + } + } + // The head car calls this when they're at the end of the lane Queued. pub fn can_start_turn( &self, @@ -17,21 +24,41 @@ impl IntersectionController { turn: TurnID, queues: &BTreeMap, time: Duration, + map: &Map, ) -> bool { - if self.accepted.is_some() { + // Policy: only one turn at a time, can't go until the target lane has room. + /*if !self.accepted.is_empty() { return false; } + // TODO This isn't strong enough -- make sure there's room for the car to immediately + // complete the turn and get out of the intersection completely. if !queues[&Traversable::Lane(turn.dst)].room_at_end(time) { return false; + }*/ + + // Policy: allow concurrent turns that don't conflict, don't prevent target lane from + // spilling over. + let req_turn = map.get_t(turn); + if self + .accepted + .iter() + .any(|(_, t)| map.get_t(*t).conflicts_with(req_turn)) + { + return false; } + true } pub fn nobody_headed_towards(&self, dst_lane: LaneID) -> bool { - if let Some((_, turn)) = self.accepted { - turn.dst != dst_lane - } else { - true - } + !self.accepted.iter().any(|(_, turn)| turn.dst == dst_lane) + } + + pub fn turn_started(&mut self, car: CarID, turn: TurnID) { + self.accepted.insert((car, turn)); + } + + pub fn turn_finished(&mut self, car: CarID, turn: TurnID) { + assert!(self.accepted.remove(&(car, turn))); } } diff --git a/editor/src/plugins/sim/new_des_model/mod.rs b/editor/src/plugins/sim/new_des_model/mod.rs index 55c38d54fc..bb2d211755 100644 --- a/editor/src/plugins/sim/new_des_model/mod.rs +++ b/editor/src/plugins/sim/new_des_model/mod.rs @@ -55,13 +55,9 @@ impl World { } for i in map.all_intersections() { - world.intersections.insert( - i.id, - IntersectionController { - id: i.id, - accepted: None, - }, - ); + world + .intersections + .insert(i.id, IntersectionController::new(i.id)); } world @@ -225,20 +221,19 @@ impl World { // Carry out the transitions. for from in head_cars_ready_to_advance { let car_id = self.queues[&from].cars[0].id; - match self.queues[&from].cars[0].path[1] { - Traversable::Turn(t) => { - if !self.intersections[&t.parent].can_start_turn(car_id, t, &self.queues, time) - { - continue; - } + let goto = self.queues[&from].cars[0].path[1]; + + // Always need to do this check. + if !self.queues[&goto].room_at_end(time) { + continue; + } + + if let Traversable::Turn(t) = goto { + if !self.intersections[&t.parent].can_start_turn(car_id, t, &self.queues, time, map) + { + continue; } - // Depending on gridlock avoidance, this could happen or not. - Traversable::Lane(l) => { - if !self.queues[&Traversable::Lane(l)].room_at_end(time) { - continue; - } - } - }; + } let mut car = self .queues @@ -251,8 +246,6 @@ impl World { car.last_steps.push_front(last_step); car.trim_last_steps(map); - let goto = car.path[0]; - let dist_int = DistanceInterval { start: Distance::ZERO, end: if car.path.len() == 1 { @@ -273,15 +266,19 @@ impl World { match goto { Traversable::Turn(t) => { - self.intersections.get_mut(&t.parent).unwrap().accepted = - Some((car.id, goto.as_turn())); - } - Traversable::Lane(_) => { self.intersections - .get_mut(&last_step.as_turn().parent) + .get_mut(&t.parent) .unwrap() - .accepted = None; + .turn_started(car.id, goto.as_turn()); } + // TODO Actually, don't call turn_finished until the car is at least vehicle_len + + // FOLLOWING_DISTANCE into the next lane. This'll be hard to predict when we're + // event-based, so hold off on this bit of realism. + Traversable::Lane(_) => self + .intersections + .get_mut(&last_step.as_turn().parent) + .unwrap() + .turn_finished(car.id, last_step.as_turn()), } self.queues.get_mut(&goto).unwrap().cars.push_back(car); @@ -300,7 +297,7 @@ impl World { .nobody_headed_towards(first_lane) { if let Some(idx) = self.queues[&Traversable::Lane(first_lane)] - .get_idx_to_insert_car(start_dist, time) + .get_idx_to_insert_car(start_dist, vehicle_len, time) { let dist_int = DistanceInterval { start: start_dist, diff --git a/editor/src/plugins/sim/new_des_model/queue.rs b/editor/src/plugins/sim/new_des_model/queue.rs index 064355cc0a..98a488d684 100644 --- a/editor/src/plugins/sim/new_des_model/queue.rs +++ b/editor/src/plugins/sim/new_des_model/queue.rs @@ -6,7 +6,7 @@ use std::collections::VecDeque; pub struct Queue { pub id: Traversable, pub cars: VecDeque, - pub max_capacity: usize, + max_capacity: usize, pub geom_len: Distance, } @@ -58,7 +58,12 @@ impl Queue { validate_positions(result, time, self.id) } - pub fn get_idx_to_insert_car(&self, start_dist: Distance, time: Duration) -> Option { + pub fn get_idx_to_insert_car( + &self, + start_dist: Distance, + vehicle_len: Distance, + time: Duration, + ) -> Option { if self.cars.len() == self.max_capacity { return None; } @@ -80,8 +85,7 @@ impl Queue { return None; } // Or the follower? - if idx != dists.len() && start_dist - MAX_VEHICLE_LENGTH - FOLLOWING_DISTANCE < dists[idx].1 - { + if idx != dists.len() && start_dist - vehicle_len - FOLLOWING_DISTANCE < dists[idx].1 { return None; } @@ -90,7 +94,7 @@ impl Queue { pub fn room_at_end(&self, time: Duration) -> bool { match self.get_car_positions(time).last() { - Some((_, front)) => *front >= MAX_VEHICLE_LENGTH + FOLLOWING_DISTANCE, + Some((car, front)) => *front >= car.vehicle_len + FOLLOWING_DISTANCE, None => true, } }