1use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet};
2
3use serde::{Deserialize, Serialize};
4
5use abstutil::{deserialize_btreemap, prettyprint_usize, serialize_btreemap, FixedMap};
6use geom::{Duration, Time};
7use map_model::{
8 ControlStopSign, ControlTrafficSignal, Intersection, IntersectionID, LaneID, Map, StageType,
9 Traversable, TurnID, TurnPriority, TurnType, UberTurn,
10};
11
12use crate::mechanics::car::{Car, CarState};
13use crate::mechanics::Queue;
14use crate::{
15 AgentID, AlertLocation, CarID, Command, DelayCause, Event, Scheduler, SimOptions, Speed,
16};
17
18const WAIT_AT_STOP_SIGN: Duration = Duration::const_seconds(0.5);
19const WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL: Duration = Duration::const_seconds(0.2);
20
21#[derive(Serialize, Deserialize, Clone)]
30pub(crate) struct IntersectionSimState {
31 state: BTreeMap<IntersectionID, State>,
32 use_freeform_policy_everywhere: bool,
33 dont_block_the_box: bool,
34 break_turn_conflict_cycles: bool,
35 handle_uber_turns: bool,
36 disable_turn_conflicts: bool,
37 blocked_by: BTreeSet<(CarID, CarID)>,
40 events: Vec<Event>,
41
42 total_repeat_requests: usize,
46 not_allowed_requests: usize,
47 blocked_by_someone_requests: usize,
48}
49
50#[derive(Clone, Debug, Serialize, Deserialize)]
51struct State {
52 id: IntersectionID,
53 accepted: BTreeSet<Request>,
55 #[serde(
58 serialize_with = "serialize_btreemap",
59 deserialize_with = "deserialize_btreemap"
60 )]
61 waiting: BTreeMap<Request, (Time, bool)>,
62 reserved: BTreeSet<Request>,
66 uber_turn_neighbors: Vec<IntersectionID>,
69
70 #[serde(
76 serialize_with = "serialize_btreemap",
77 deserialize_with = "deserialize_btreemap"
78 )]
79 leader_eta: BTreeMap<LaneID, (Request, Time)>,
80
81 signal: Option<SignalState>,
82}
83
84#[derive(Clone, Debug, Serialize, Deserialize)]
85struct SignalState {
86 current_stage: usize,
88 stage_ends_at: Time,
90 extensions_count: usize,
92}
93
94#[derive(PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize, Clone, Debug)]
95struct Request {
96 agent: AgentID,
97 turn: TurnID,
98}
99
100impl IntersectionSimState {
102 pub fn new(map: &Map, scheduler: &mut Scheduler, opts: &SimOptions) -> IntersectionSimState {
103 let mut sim = IntersectionSimState {
104 state: BTreeMap::new(),
105 use_freeform_policy_everywhere: opts.use_freeform_policy_everywhere,
106 dont_block_the_box: !opts.allow_block_the_box,
107 break_turn_conflict_cycles: !opts.dont_break_turn_conflict_cycles,
108 handle_uber_turns: !opts.dont_handle_uber_turns,
109 disable_turn_conflicts: opts.disable_turn_conflicts,
110 blocked_by: BTreeSet::new(),
111 events: Vec::new(),
112
113 total_repeat_requests: 0,
114 not_allowed_requests: 0,
115 blocked_by_someone_requests: 0,
116 };
117 if sim.disable_turn_conflicts {
118 sim.use_freeform_policy_everywhere = true;
119 }
120
121 for i in map.all_intersections() {
122 let mut state = State {
123 id: i.id,
124 accepted: BTreeSet::new(),
125 waiting: BTreeMap::new(),
126 reserved: BTreeSet::new(),
127 uber_turn_neighbors: Vec::new(),
128 signal: None,
129 leader_eta: BTreeMap::new(),
130 };
131 if i.is_traffic_signal() {
132 state.signal = Some(SignalState::new(i.id, Time::START_OF_DAY, map, scheduler));
133 }
134 if let Some(mut set) = map_model::IntersectionCluster::autodetect(i.id, map) {
135 set.remove(&i.id);
136 state.uber_turn_neighbors.extend(set);
137 }
138 sim.state.insert(i.id, state);
139 }
140 sim
141 }
142
143 pub fn turn_finished(
144 &mut self,
145 now: Time,
146 agent: AgentID,
147 turn: TurnID,
148 scheduler: &mut Scheduler,
149 map: &Map,
150 handling_live_edits: bool,
151 ) {
152 let state = self.state.get_mut(&turn.parent).unwrap();
153 assert!(state.accepted.remove(&Request { agent, turn }));
154
155 state.reserved.remove(&Request { agent, turn });
156 if !handling_live_edits && map.get_t(turn).turn_type != TurnType::SharedSidewalkCorner {
157 self.wakeup_waiting(now, turn.parent, scheduler, map);
158 }
159 if self.break_turn_conflict_cycles {
160 if let AgentID::Car(car) = agent {
161 self.blocked_by.retain(|(_, c)| *c != car);
162 }
163 }
164
165 for id in &self.state[&turn.parent].uber_turn_neighbors {
170 self.wakeup_waiting(now, *id, scheduler, map);
171 }
172 }
173
174 pub fn cancel_request(&mut self, agent: AgentID, turn: TurnID) {
176 let state = self.state.get_mut(&turn.parent).unwrap();
177 state.waiting.remove(&Request { agent, turn });
178 if self.break_turn_conflict_cycles {
179 if let AgentID::Car(car) = agent {
180 self.blocked_by.retain(|(c1, c2)| *c1 != car && *c2 != car);
181 }
182 }
183 }
184
185 pub fn space_freed(
186 &mut self,
187 now: Time,
188 i: IntersectionID,
189 scheduler: &mut Scheduler,
190 map: &Map,
191 ) {
192 self.wakeup_waiting(now, i, scheduler, map);
193 }
194
195 pub fn vehicle_gone(&mut self, car: CarID) {
198 self.blocked_by.retain(|(c1, c2)| *c1 != car && *c2 != car);
199 }
200
201 pub fn agent_deleted_mid_turn(&mut self, agent: AgentID, turn: TurnID) {
202 let state = self.state.get_mut(&turn.parent).unwrap();
203 assert!(state.accepted.remove(&Request { agent, turn }));
204
205 for state in self.state.values_mut() {
208 state.reserved.retain(|req| req.agent != agent);
209 }
210 }
211
212 fn wakeup_waiting(&self, now: Time, i: IntersectionID, scheduler: &mut Scheduler, map: &Map) {
213 let mut all: Vec<(Request, Time, bool)> = self.state[&i]
214 .waiting
215 .iter()
216 .map(|(r, (t, urgent))| (r.clone(), *t, *urgent))
217 .collect();
218 all.sort_by_key(|(_, t, urgent)| (!*urgent, *t));
222
223 let mut protected = Vec::new();
227 let mut yielding = Vec::new();
228
229 if self.use_freeform_policy_everywhere {
230 for (req, _, _) in all {
231 protected.push(req);
232 }
233 } else if let Some(signal) = map.maybe_get_traffic_signal(i) {
234 let current_stage = self.state[&i].signal.as_ref().unwrap().current_stage;
235 let stage = &signal.stages[current_stage];
236 let reserved = &self.state[&i].reserved;
237 let i = map.get_i(i);
238 for (req, _, _) in all {
239 match stage.get_priority_of_turn(req.turn, i) {
240 TurnPriority::Protected => {
241 protected.push(req);
242 }
243 TurnPriority::Yield => {
244 yielding.push(req);
245 }
246 TurnPriority::Banned => {
248 if reserved.contains(&req) {
249 protected.push(req);
250 }
251 }
252 }
253 }
254 } else if let Some(sign) = map.maybe_get_stop_sign(i) {
255 for (req, _, _) in all {
256 match sign.get_priority(req.turn, map) {
257 TurnPriority::Protected => {
258 protected.push(req);
259 }
260 TurnPriority::Yield => {
261 yielding.push(req);
262 }
263 TurnPriority::Banned => unreachable!(),
264 }
265 }
266 } else {
267 assert!(protected.is_empty());
271 assert!(yielding.is_empty());
272 };
273 protected.extend(yielding);
274
275 let mut delay = Duration::ZERO;
279 for req in protected {
280 scheduler.update(now + delay, Command::update_agent(req.agent));
283 delay += Duration::EPSILON;
284 }
285 }
286
287 pub fn update_intersection(
289 &mut self,
290 now: Time,
291 id: IntersectionID,
292 map: &Map,
293 scheduler: &mut Scheduler,
294 ) {
295 let i = map.get_i(id);
296
297 fn advance(
299 signal_state: &mut SignalState,
300 signal: &ControlTrafficSignal,
301 i: &Intersection,
302 allow_crosswalk_skip: bool,
303 ) -> Duration {
304 signal_state.current_stage = (signal_state.current_stage + 1) % signal.stages.len();
305 let stage = &signal.stages[signal_state.current_stage];
306 if let StageType::Variable(_, _, _) = stage.stage_type {
308 if allow_crosswalk_skip && stage.max_crosswalk_time(i).is_some() {
309 signal_state.current_stage =
312 (signal_state.current_stage + 1) % signal.stages.len();
313 }
314 }
315 signal.stages[signal_state.current_stage]
316 .stage_type
317 .simple_duration()
318 }
319 let state = self.state.get_mut(&id).unwrap();
320 let signal_state = state.signal.as_mut().unwrap();
321 let signal = map.get_traffic_signal(id);
322 let ped_waiting = state.waiting.keys().any(|req| {
323 if let AgentID::Pedestrian(_) = req.agent {
324 return true;
325 }
326 false
327 });
328 let duration: Duration;
329 assert_eq!(now, signal_state.stage_ends_at);
331 let old_stage = &signal.stages[signal_state.current_stage];
332 match old_stage.stage_type {
333 StageType::Fixed(_) => {
334 duration = advance(signal_state, signal, i, !ped_waiting);
335 }
336 StageType::Variable(min, delay, additional) => {
337 let delay = std::cmp::max(Duration::const_seconds(1.0), delay);
341 if signal_state.extensions_count as f64 * delay.inner_seconds()
343 >= additional.inner_seconds()
344 {
345 self.events.push(Event::Alert(
346 AlertLocation::Intersection(id),
347 format!(
348 "exhausted a variable stage {},{},{},{}",
349 min, delay, additional, signal_state.extensions_count
350 ),
351 ));
352 duration = advance(signal_state, signal, i, !ped_waiting);
353 signal_state.extensions_count = 0;
354 } else if state.waiting.keys().all(|req| {
355 if let AgentID::Pedestrian(_) = req.agent {
356 return true;
357 }
358 old_stage.get_priority_of_turn(req.turn, i) != TurnPriority::Protected
361 }) {
362 signal_state.extensions_count = 0;
363 duration = advance(signal_state, signal, i, !ped_waiting);
364 } else {
365 signal_state.extensions_count += 1;
366 duration = delay;
367 self.events.push(Event::Alert(
368 AlertLocation::Intersection(id),
369 format!(
370 "Extending a variable stage {},{},{},{}",
371 min, delay, additional, signal_state.extensions_count
372 ),
373 ));
374 }
375 }
376 }
377
378 signal_state.stage_ends_at = now + duration;
379 scheduler.push(signal_state.stage_ends_at, Command::UpdateIntersection(id));
380 self.wakeup_waiting(now, id, scheduler, map);
381 }
382
383 pub fn maybe_start_turn(
391 &mut self,
392 agent: AgentID,
393 turn: TurnID,
394 speed: Speed,
395 now: Time,
396 map: &Map,
397 scheduler: &mut Scheduler,
398 maybe_cars_and_queues: Option<(
399 &Car,
400 &FixedMap<CarID, Car>,
401 &mut HashMap<Traversable, Queue>,
402 )>,
403 ) -> bool {
404 #![allow(clippy::logic_bug)] let req = Request { agent, turn };
406
407 if let Some(_eta) = self
408 .state
409 .get_mut(&turn.parent)
410 .unwrap()
411 .leader_eta
412 .remove(&req.turn.src)
413 {
414 }
418
419 let entry = self
420 .state
421 .get_mut(&turn.parent)
422 .unwrap()
423 .waiting
424 .entry(req.clone());
425 let repeat_request = match entry {
426 std::collections::btree_map::Entry::Vacant(_) => false,
427 std::collections::btree_map::Entry::Occupied(_) => true,
428 };
429 let urgent = if let Some((car, _, queues)) = maybe_cars_and_queues.as_ref() {
430 queues[&car.router.head()].is_overflowing()
431 } else {
432 false
433 };
434 entry.or_insert((now, urgent));
435
436 if repeat_request {
437 self.total_repeat_requests += 1;
438 }
439
440 let shared_sidewalk_corner =
441 map.get_t(req.turn).turn_type == TurnType::SharedSidewalkCorner;
442
443 let readonly_pair = maybe_cars_and_queues.as_ref().map(|(_, c, q)| (*c, &**q));
444 let started_uber_turn = |state: &Self, car: &Car| {
445 state.handle_uber_turns && car.router.get_path().currently_inside_ut().is_some()
446 };
447 #[allow(clippy::if_same_then_else)]
448 let allowed = if shared_sidewalk_corner {
449 true
451 } else if !self.handle_accepted_conflicts(&req, map, readonly_pair, Some((now, scheduler)))
452 {
453 false
455 } else if maybe_cars_and_queues
456 .as_ref()
457 .map(|(car, _, _)| started_uber_turn(self, *car))
458 .unwrap_or(false)
459 {
460 if let Some(signal) = map.maybe_get_traffic_signal(turn.parent) {
463 if !self.traffic_signal_policy(&req, map, signal, speed, now, None) && false {
465 self.events.push(Event::Alert(
466 AlertLocation::Intersection(req.turn.parent),
467 format!("Running a red light inside an uber-turn: {:?}", req),
468 ));
469 }
470 }
471
472 true
473 } else if self.use_freeform_policy_everywhere {
474 true
476 } else if let Some(signal) = map.maybe_get_traffic_signal(turn.parent) {
477 self.traffic_signal_policy(&req, map, signal, speed, now, Some(scheduler))
478 } else if let Some(sign) = map.maybe_get_stop_sign(turn.parent) {
479 self.stop_sign_policy(&req, map, sign, speed, now, scheduler)
480 } else {
481 unreachable!()
482 };
483 if !allowed {
484 if repeat_request {
485 self.not_allowed_requests += 1;
486 }
487 if self.handle_uber_turns {
489 if let Some(ut) = maybe_cars_and_queues
490 .as_ref()
491 .and_then(|(car, _, _)| car.router.get_path().about_to_start_ut())
492 {
493 for t in &ut.path {
494 self.state
495 .get_mut(&t.parent)
496 .unwrap()
497 .reserved
498 .remove(&Request { agent, turn: *t });
499 }
500 }
501 }
502 return false;
503 }
504
505 if self.handle_uber_turns {
507 if let Some(ut) = maybe_cars_and_queues
508 .as_ref()
509 .and_then(|(car, _, _)| car.router.get_path().about_to_start_ut())
510 {
511 for t in &ut.path {
513 let req = Request { agent, turn: *t };
514 if !self.handle_accepted_conflicts(&req, map, readonly_pair, None) {
515 if repeat_request {
516 self.blocked_by_someone_requests += 1;
517 }
518 return false;
519 }
520 }
521 for t in &ut.path {
523 self.state
524 .get_mut(&t.parent)
525 .unwrap()
526 .reserved
527 .insert(Request { agent, turn: *t });
528 }
529 }
530 }
531
532 if let Some((car, cars, queues)) = maybe_cars_and_queues {
534 assert_eq!(agent, AgentID::Car(car.vehicle.id));
535 let inside_ut = self.handle_uber_turns
536 && (car.router.get_path().currently_inside_ut().is_some()
537 || car.router.get_path().about_to_start_ut().is_some());
538 let queue = queues.get_mut(&Traversable::Lane(turn.dst)).unwrap();
539 if !queue.try_to_reserve_entry(
540 car,
541 !self.dont_block_the_box
542 || allow_block_the_box(map.get_i(turn.parent))
543 || inside_ut,
544 ) {
545 let mut actually_did_reserve_entry = false;
546 if self.break_turn_conflict_cycles {
547 if let Some(c) = queue.laggy_head {
548 self.blocked_by.insert((car.vehicle.id, c));
549 } else if let Some(c) = queue.get_active_cars().get(0) {
550 self.blocked_by.insert((car.vehicle.id, *c));
551 } else {
552 let blocking_req = self.state[&turn.parent]
556 .accepted
557 .iter()
558 .find(|r| r.turn.dst == turn.dst)
559 .unwrap();
560 self.blocked_by
561 .insert((car.vehicle.id, blocking_req.agent.as_car()));
562 }
563
564 if self
566 .detect_conflict_cycle(car.vehicle.id, (cars, queues))
567 .is_some()
568 {
569 let queue = queues.get_mut(&Traversable::Lane(turn.dst)).unwrap();
571 actually_did_reserve_entry = queue.try_to_reserve_entry(car, true);
572 }
573 }
574
575 if !actually_did_reserve_entry {
576 if repeat_request {
577 self.blocked_by_someone_requests += 1;
578 }
579 return false;
580 }
581 }
582 }
583
584 let state = self.state.get_mut(&turn.parent).unwrap();
587 state.waiting.remove(&req).unwrap();
588 state.accepted.insert(req);
589 if self.break_turn_conflict_cycles {
590 if let AgentID::Car(car) = agent {
591 self.blocked_by.retain(|(c, _)| *c != car);
592 }
593 }
594 true
595 }
596
597 pub fn collect_events(&mut self) -> Vec<Event> {
598 std::mem::take(&mut self.events)
599 }
600
601 pub fn handle_live_edited_traffic_signals(
602 &mut self,
603 now: Time,
604 map: &Map,
605 scheduler: &mut Scheduler,
606 ) {
607 for state in self.state.values_mut() {
608 match (
609 map.maybe_get_traffic_signal(state.id),
610 state.signal.as_mut(),
611 ) {
612 (Some(ts), Some(signal_state)) => {
613 if signal_state.current_stage >= ts.stages.len() {
614 signal_state.current_stage = 0;
616 println!(
617 "WARNING: Traffic signal {} was live-edited in the middle of a stage, \
618 so jumping back to the first stage",
619 state.id
620 );
621 }
622 }
623 (Some(_), None) => {
624 state.signal = Some(SignalState::new(state.id, now, map, scheduler));
625 }
626 (None, Some(_)) => {
627 state.signal = None;
628 scheduler.cancel(Command::UpdateIntersection(state.id));
629 }
630 (None, None) => {}
631 }
632
633 state.uber_turn_neighbors.clear();
636 if let Some(mut set) = map_model::IntersectionCluster::autodetect(state.id, map) {
637 set.remove(&state.id);
638 state.uber_turn_neighbors.extend(set);
639 }
640 }
641 }
642
643 pub fn handle_live_edits(&self, map: &Map) {
644 let mut errors = Vec::new();
646 for state in self.state.values() {
647 for req in &state.accepted {
648 if map.maybe_get_t(req.turn).is_none() {
649 errors.push(format!("{} accepted for {}", req.agent, req.turn));
650 }
651 }
652 for req in state.waiting.keys() {
653 if map.maybe_get_t(req.turn).is_none() {
654 errors.push(format!("{} waiting for {}", req.agent, req.turn));
655 }
656 }
657 for req in &state.reserved {
658 if map.maybe_get_t(req.turn).is_none() {
659 errors.push(format!("{} has reserved {}", req.agent, req.turn));
660 }
661 }
662 }
663 if !errors.is_empty() {
664 for x in errors {
665 error!("{}", x);
666 }
667 panic!("After live map edits, intersection state refers to deleted turns!");
668 }
669 }
670
671 pub fn approaching_leader(&mut self, agent: AgentID, turn: TurnID, eta: Time) {
676 let state = self.state.get_mut(&turn.parent).unwrap();
677 state
680 .leader_eta
681 .insert(turn.src, (Request { agent, turn }, eta));
682 }
683}
684
685impl IntersectionSimState {
687 pub fn nobody_headed_towards(&self, lane: LaneID, i: IntersectionID) -> bool {
688 let state = &self.state[&i];
689 !state
690 .accepted
691 .iter()
692 .chain(state.reserved.iter())
693 .any(|req| req.turn.dst == lane)
694 }
695
696 pub fn debug_json(&self, id: IntersectionID, map: &Map) -> String {
697 let json1 = abstutil::to_json(&self.state[&id]);
698 let json2 = if let Some(ref sign) = map.maybe_get_stop_sign(id) {
699 abstutil::to_json(sign)
700 } else if let Some(ref signal) = map.maybe_get_traffic_signal(id) {
701 abstutil::to_json(signal)
702 } else {
703 "\"Border\"".to_string()
704 };
705 format!("[{json1}, {json2}]")
706 }
707
708 pub fn get_accepted_agents(&self, id: IntersectionID) -> Vec<(AgentID, TurnID)> {
709 self.state[&id]
710 .accepted
711 .iter()
712 .map(|req| (req.agent, req.turn))
713 .collect()
714 }
715
716 pub fn get_waiting_agents(&self, id: IntersectionID) -> Vec<(AgentID, TurnID, Time)> {
717 self.state[&id]
718 .waiting
719 .iter()
720 .map(|(req, (time, _))| (req.agent, req.turn, *time))
721 .collect()
722 }
723
724 pub fn delayed_intersections(
727 &self,
728 now: Time,
729 threshold: Duration,
730 ) -> Vec<(IntersectionID, Time)> {
731 let mut candidates = Vec::new();
732 for state in self.state.values() {
733 if let Some((earliest, _)) = state.waiting.values().min() {
734 if now - *earliest >= threshold {
735 candidates.push((state.id, *earliest));
736 }
737 }
738 }
739 candidates.sort_by_key(|(_, t)| *t);
740 candidates
741 }
742
743 pub fn current_stage_and_remaining_time(
744 &self,
745 now: Time,
746 i: IntersectionID,
747 ) -> (usize, Duration) {
748 let state = &self.state[&i].signal.as_ref().unwrap();
749 if now > state.stage_ends_at {
750 panic!(
751 "At {}, but {} should have advanced its stage at {}",
752 now, i, state.stage_ends_at
753 );
754 }
755 (state.current_stage, state.stage_ends_at - now)
756 }
757
758 pub fn describe_stats(&self) -> Vec<String> {
759 vec![
760 "intersection stats".to_string(),
761 format!(
762 "{} total turn requests repeated after the initial attempt",
763 prettyprint_usize(self.total_repeat_requests)
764 ),
765 format!(
766 "{} not allowed by intersection ({}%)",
767 prettyprint_usize(self.not_allowed_requests),
768 (100.0 * (self.not_allowed_requests as f64) / (self.total_repeat_requests as f64))
769 .round()
770 ),
771 format!(
772 "{} blocked by someone in the way ({}%)",
773 prettyprint_usize(self.blocked_by_someone_requests),
774 (100.0 * (self.blocked_by_someone_requests as f64)
775 / (self.total_repeat_requests as f64))
776 .round()
777 ),
778 ]
779 }
780
781 pub fn populate_blocked_by(
782 &self,
783 now: Time,
784 graph: &mut BTreeMap<AgentID, (Duration, DelayCause)>,
785 map: &Map,
786 cars: &FixedMap<CarID, Car>,
787 queues: &HashMap<Traversable, Queue>,
788 ) {
789 for state in self.state.values() {
793 for (req, (started_at, _)) in &state.waiting {
794 let turn = map.get_t(req.turn);
795 let mut cause = DelayCause::Intersection(state.id);
800 if let Some(other) = state.accepted.iter().find(|other| {
801 turn.conflicts_with(map.get_t(other.turn)) || turn.id == other.turn
802 }) {
803 cause = DelayCause::Agent(other.agent);
804 } else if let AgentID::Car(car) = req.agent {
805 let queue = &queues[&Traversable::Lane(req.turn.dst)];
806 let car = cars.get(&car).unwrap();
807 if !queue.room_for_car(car) {
808 let blocker = queue
810 .get_active_cars()
811 .last()
812 .cloned()
813 .or(queue.laggy_head)
814 .unwrap();
815 cause = DelayCause::Agent(AgentID::Car(blocker));
816 } else if let Some(ut) = car.router.get_path().about_to_start_ut() {
817 if let Some(blocker) = self.check_for_conflicts_before_uber_turn(ut, map) {
818 cause = DelayCause::Agent(blocker);
819 }
820 }
821 }
822 graph.insert(req.agent, (now - *started_at, cause));
823 }
824 }
825 }
826
827 fn check_for_conflicts_before_uber_turn(&self, ut: &UberTurn, map: &Map) -> Option<AgentID> {
830 for t in &ut.path {
831 let turn = map.get_t(*t);
832 let state = &self.state[&turn.id.parent];
833 for other in state.accepted.iter().chain(state.reserved.iter()) {
834 if map.get_t(other.turn).conflicts_with(turn) {
835 return Some(other.agent);
836 }
837 }
838 }
839 None
840 }
841}
842
843impl IntersectionSimState {
845 fn stop_sign_policy(
846 &mut self,
847 req: &Request,
848 map: &Map,
849 sign: &ControlStopSign,
850 speed: Speed,
851 now: Time,
852 scheduler: &mut Scheduler,
853 ) -> bool {
854 let our_priority = sign.get_priority(req.turn, map);
855 assert!(our_priority != TurnPriority::Banned);
856 let (our_time, _) = self.state[&req.turn.parent].waiting[req];
857
858 if our_priority == TurnPriority::Yield && now < our_time + WAIT_AT_STOP_SIGN {
859 scheduler.push(
862 our_time + WAIT_AT_STOP_SIGN,
863 Command::update_agent(req.agent),
864 );
865 return false;
866 }
867
868 if req.agent.is_pedestrian() {
890 let our_turn = map.get_t(req.turn);
891 let time_to_cross = our_turn.geom.length() / speed;
892 for (other_req, (other_time, _)) in &self.state[&req.turn.parent].waiting {
893 if matches!(other_req.agent, AgentID::Car(_)) {
894 if our_turn.conflicts_with(map.get_t(other_req.turn)) {
895 let our_waiting = now - our_time;
896 let other_waiting = now - *other_time;
897 if our_waiting > other_waiting {
901 continue;
902 }
903 if other_waiting > time_to_cross + Duration::seconds(3.0) {
908 return false;
909 }
910 }
911 }
912 }
913 }
914
915 true
916 }
917
918 fn traffic_signal_policy(
919 &mut self,
920 req: &Request,
921 map: &Map,
922 signal: &ControlTrafficSignal,
923 speed: Speed,
924 now: Time,
925 scheduler: Option<&mut Scheduler>,
926 ) -> bool {
927 let turn = map.get_t(req.turn);
928
929 let state = &self.state[&req.turn.parent];
930 let signal_state = state.signal.as_ref().unwrap();
931 let stage = &signal.stages[signal_state.current_stage];
932 let full_stage_duration = stage.stage_type.simple_duration();
933 let remaining_stage_time = signal_state.stage_ends_at - now;
934 let (our_time, _) = state.waiting[req];
935
936 let our_priority = stage.get_priority_of_turn(req.turn, map.get_i(state.id));
938 if our_priority == TurnPriority::Banned {
939 return false;
940 }
941
942 if our_priority == TurnPriority::Yield
943 && now < our_time + WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL
944 {
945 if let Some(s) = scheduler {
948 s.push(
949 our_time + WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL,
950 Command::update_agent(req.agent),
951 );
952 }
953 return false;
954 }
955
956 let time_to_cross = turn.geom.length() / speed;
968 if time_to_cross > remaining_stage_time {
969 if time_to_cross <= full_stage_duration {
973 return false;
974 }
975 }
976
977 true
978 }
979
980 fn handle_accepted_conflicts(
982 &mut self,
983 req: &Request,
984 map: &Map,
985 maybe_cars_and_queues: Option<(&FixedMap<CarID, Car>, &HashMap<Traversable, Queue>)>,
986 wakeup_stuck_cycle: Option<(Time, &mut Scheduler)>,
987 ) -> bool {
988 let turn = map.get_t(req.turn);
989 let mut cycle_detected = false;
990 let mut ok = true;
991 for other in self.state[&req.turn.parent]
992 .accepted
993 .iter()
994 .chain(self.state[&req.turn.parent].reserved.iter())
995 {
996 if map.get_t(other.turn).conflicts_with(turn) {
999 if self.break_turn_conflict_cycles {
1000 if let AgentID::Car(c) = req.agent {
1001 if let AgentID::Car(c2) = other.agent {
1002 self.blocked_by.insert((c, c2));
1003 }
1004 if !cycle_detected {
1005 if let Some(cycle) =
1006 self.detect_conflict_cycle(c, maybe_cars_and_queues.unwrap())
1007 {
1008 self.events.push(Event::Alert(
1010 AlertLocation::Intersection(req.turn.parent),
1011 format!(
1012 "{} found turn conflict cycle involving {:?}",
1013 req.agent, cycle
1014 ),
1015 ));
1016 cycle_detected = true;
1017 }
1018 }
1019 }
1020 }
1021
1022 if !cycle_detected && !self.disable_turn_conflicts {
1023 ok = false;
1024 }
1025
1026 if turn.id.dst == other.turn.dst {
1031 if let (Some((now, scheduler)), AgentID::Car(blocker), Some((cars, _))) = (
1032 wakeup_stuck_cycle,
1033 other.agent,
1034 maybe_cars_and_queues.as_ref(),
1035 ) {
1036 if cycle_detected
1039 && matches!(cars[&blocker].state, CarState::WaitingToAdvance { .. })
1040 {
1041 self.events.push(Event::Alert(
1042 AlertLocation::Intersection(req.turn.parent),
1043 format!(
1044 "{} waking up {}, who's blocking it as part of a cycle",
1045 req.agent, other.agent
1046 ),
1047 ));
1048 scheduler.update(
1049 now + Duration::EPSILON,
1050 Command::update_agent(other.agent),
1051 );
1052 }
1053 }
1054 return false;
1055 }
1056 }
1057 }
1058 ok
1059 }
1060
1061 fn detect_conflict_cycle(
1062 &self,
1063 car: CarID,
1064 pair: (&FixedMap<CarID, Car>, &HashMap<Traversable, Queue>),
1065 ) -> Option<HashSet<CarID>> {
1066 let (cars, queues) = pair;
1067
1068 let mut queue = vec![car];
1069 let mut seen = HashSet::new();
1070 while !queue.is_empty() {
1071 let current = queue.pop().unwrap();
1072 if !seen.is_empty() && current == car {
1075 return Some(seen);
1076 }
1077 if !seen.contains(¤t) {
1078 seen.insert(current);
1079
1080 for (c1, c2) in &self.blocked_by {
1081 if *c1 == current {
1082 queue.push(*c2);
1083 }
1084 }
1085
1086 if current != car {
1090 let q = &queues[&cars[¤t].router.head()];
1091 let head = if let Some(c) = q.laggy_head {
1092 c
1093 } else {
1094 q.get_active_cars()[0]
1095 };
1096 if current != head {
1097 queue.push(head);
1098 }
1099 }
1100 }
1101 }
1102 None
1103 }
1104}
1105
1106impl SignalState {
1107 fn new(id: IntersectionID, now: Time, map: &Map, scheduler: &mut Scheduler) -> SignalState {
1108 let mut state = SignalState {
1109 current_stage: 0,
1110 stage_ends_at: now,
1111 extensions_count: 0,
1112 };
1113
1114 let signal = map.get_traffic_signal(id);
1115 let mut offset = (now - Time::START_OF_DAY) + signal.offset;
1117 loop {
1118 let dt = signal.stages[state.current_stage]
1119 .stage_type
1120 .simple_duration();
1121 if offset >= dt {
1122 offset -= dt;
1123 state.current_stage += 1;
1124 if state.current_stage == signal.stages.len() {
1125 state.current_stage = 0;
1126 }
1127 } else {
1128 state.stage_ends_at = now + dt - offset;
1129 break;
1130 }
1131 }
1132 scheduler.push(state.stage_ends_at, Command::UpdateIntersection(id));
1133 state
1134 }
1135}
1136
1137fn allow_block_the_box(i: &Intersection) -> bool {
1138 if i.roads.len() == 2 {
1143 return true;
1144 }
1145
1146 let id = i.orig_id.0;
1154 if id == 53211693
1156 || id == 53214134
1157 || id == 53214133
1158 || id == 987334546
1159 || id == 848817336
1160 || id == 1726088131
1161 || id == 1726088130
1162 || id == 53217946
1163 || id == 53223864
1164 || id == 53211694
1165 || id == 5440360144
1166 || id == 246768814
1167 {
1168 return true;
1169 }
1170 if id == 18030505 || id == 2124133018 || id == 30024649 {
1172 return true;
1173 }
1174 false
1175}