1use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet, VecDeque};
2
3use serde::{Deserialize, Serialize};
4
5use abstutil::{deserialize_hashmap, serialize_hashmap, FixedMap, IndexableKey};
6use geom::{Distance, Duration, PolyLine, Time};
7use map_model::{DrivingSide, IntersectionID, LaneID, Map, Path, PathStep, Position, Traversable};
8
9use crate::mechanics::car::{Car, CarState};
10use crate::mechanics::queue::{Queue, QueueEntry, Queued};
11use crate::sim::Ctx;
12use crate::{
13 ActionAtEnd, AgentID, AgentProperties, CarID, CarStatus, Command, CreateCar, DelayCause,
14 DistanceInterval, DrawCarInput, Event, IntersectionSimState, ParkedCar, ParkingSim,
15 ParkingSpot, PersonID, Problem, SimOptions, TimeInterval, TransitSimState, TripID, TripManager,
16 UnzoomedAgent, Vehicle, VehicleType, WalkingSimState, FOLLOWING_DISTANCE, MAX_CAR_LENGTH,
17};
18
19const TIME_TO_WAIT_AT_BUS_STOP: Duration = Duration::const_seconds(10.0);
20const TIME_TO_CHANGE_LANES: Duration = Duration::const_seconds(1.0);
21
22pub const BLIND_RETRY_TO_CREEP_FORWARDS: Duration = Duration::const_seconds(0.1);
24pub const BLIND_RETRY_TO_REACH_END_DIST: Duration = Duration::const_seconds(5.0);
25
26#[derive(Serialize, Deserialize, Clone)]
28pub(crate) struct DrivingSimState {
29 cars: FixedMap<CarID, Car>,
34 #[serde(
38 serialize_with = "serialize_hashmap",
39 deserialize_with = "deserialize_hashmap"
40 )]
41 queues: HashMap<Traversable, Queue>,
42 events: Vec<Event>,
43
44 waiting_to_spawn: BTreeMap<CarID, (Position, Option<PersonID>)>,
45
46 recalc_lanechanging: bool,
47 handle_uber_turns: bool,
48
49 time_to_unpark_onstreet: Duration,
50 time_to_park_onstreet: Duration,
51 time_to_unpark_offstreet: Duration,
52 time_to_park_offstreet: Duration,
53}
54
55impl DrivingSimState {
57 pub fn new(map: &Map, opts: &SimOptions) -> DrivingSimState {
58 let mut sim = DrivingSimState {
59 cars: FixedMap::new(),
60 queues: HashMap::new(),
61 events: Vec::new(),
62 recalc_lanechanging: !opts.dont_recalc_lanechanging,
63 handle_uber_turns: !opts.dont_handle_uber_turns,
64 waiting_to_spawn: BTreeMap::new(),
65
66 time_to_unpark_onstreet: Duration::seconds(10.0),
67 time_to_park_onstreet: Duration::seconds(15.0),
68 time_to_unpark_offstreet: Duration::seconds(5.0),
69 time_to_park_offstreet: Duration::seconds(5.0),
70 };
71 if opts.infinite_parking {
72 sim.time_to_unpark_offstreet = Duration::seconds(0.1);
73 sim.time_to_park_offstreet = Duration::seconds(0.1);
74 }
75
76 for l in map.all_lanes() {
77 if l.lane_type.is_for_moving_vehicles() {
78 let q = Queue::new(Traversable::Lane(l.id), map);
79 sim.queues.insert(q.id, q);
80 }
81 }
82 for t in map.all_turns() {
83 if !t.between_sidewalks() {
84 let q = Queue::new(Traversable::Turn(t.id), map);
85 sim.queues.insert(q.id, q);
86 }
87 }
88
89 sim
90 }
91
92 pub fn start_car_on_lane(
94 &mut self,
95 now: Time,
96 mut params: CreateCar,
97 ctx: &mut Ctx,
98 ) -> Option<CreateCar> {
99 let first_lane = params.router.head().as_lane();
100 let mut start_dist = params.router.get_path().get_req().start.dist_along();
101 if let Some(ref p) = params.maybe_parked_car {
102 if !matches!(p.spot, ParkingSpot::Onstreet(_, _)) {
107 start_dist += params.vehicle.length;
108 if start_dist > ctx.map.get_l(first_lane).length() {
110 error!(
111 "At {}, {} needs to exit a driveway at {} on {}, but they overflow",
112 now, params.vehicle.id, start_dist, first_lane
113 );
114 start_dist -= params.vehicle.length;
117 }
118 }
119 }
120
121 let mut blocked_starts: Vec<(Position, usize)> = Vec::new();
124 for lane in params.router.get_path().get_blocked_starts() {
125 let pos = match params
128 .router
129 .get_path()
130 .get_req()
131 .start
132 .equiv_pos(lane, ctx.map)
133 .buffer_dist(MAX_CAR_LENGTH + FOLLOWING_DISTANCE, ctx.map)
134 {
135 Some(pos) => pos,
136 None => {
137 if false {
142 warn!("Not inserting a static blockage on {} at {} for {} to spawn, because the lane is too short", lane, now, params.vehicle.id);
143 }
144 continue;
145 }
146 };
147 if !self.queues.contains_key(&Traversable::Lane(lane)) {
148 continue;
151 }
152 if let Some(idx) = self.queues[&Traversable::Lane(lane)].can_block_from_driveway(
153 &pos,
155 params.vehicle.length,
156 now,
157 &self.cars,
158 &self.queues,
159 ) {
160 blocked_starts.push((pos, idx));
161 } else {
162 return Some(params);
163 }
164 }
167
168 if !ctx
169 .intersections
170 .nobody_headed_towards(first_lane, ctx.map.get_l(first_lane).src_i)
171 {
172 return Some(params);
173 }
174 if let Some(idx) = self.queues[&Traversable::Lane(first_lane)].get_idx_to_insert_car(
175 start_dist,
176 params.vehicle.length,
177 now,
178 &self.cars,
179 &self.queues,
180 ) {
181 let mut car = Car {
182 vehicle: params.vehicle,
183 router: params.router,
184 state: CarState::Queued {
186 blocked_since: now,
187 want_to_change_lanes: None,
188 },
189 last_steps: VecDeque::new(),
190 started_at: now,
191 total_blocked_time: Duration::ZERO,
192 trip_and_person: params.trip_and_person,
193 wants_to_overtake: BTreeSet::new(),
194 };
195 let mut start_crossing = false;
196 if let Some(p) = params.maybe_parked_car {
197 let delay = match p.spot {
198 ParkingSpot::Onstreet(_, _) => self.time_to_unpark_onstreet,
199 ParkingSpot::Offstreet(_, _) | ParkingSpot::Lot(_, _) => {
200 if blocked_starts.is_empty() {
206 self.time_to_unpark_offstreet
207 } else {
208 Duration::seconds(5.0)
209 }
210 }
211 };
212 let mut lanes = Vec::new();
213 for (pos, idx) in blocked_starts {
214 self.queues
215 .get_mut(&Traversable::Lane(pos.lane()))
216 .unwrap()
217 .add_static_blockage(
218 car.vehicle.id,
219 pos.dist_along(),
220 pos.dist_along() - car.vehicle.length,
221 idx,
222 );
223 lanes.push(pos.lane());
224 }
225
226 car.state = CarState::Unparking {
227 front: start_dist,
228 spot: p.spot,
229 time_int: TimeInterval::new(now, now + delay),
230 blocked_starts: lanes,
231 };
232 } else {
233 if car.router.last_step() {
235 match car.router.maybe_handle_end(
236 start_dist,
237 &car.vehicle,
238 ctx.parking,
239 ctx.map,
240 car.trip_and_person,
241 &mut self.events,
242 ) {
243 None | Some(ActionAtEnd::GotoLaneEnd) => {}
244 x => {
245 panic!(
246 "Car with one-step route {:?} had unexpected result from \
247 maybe_handle_end: {:?}",
248 car.router, x
249 );
250 }
251 }
252 if car.router.last_step() && start_dist > car.router.get_end_dist() {
255 println!(
256 "WARNING: {} wants to spawn at {}, which is past their end of {} on a \
257 one-step path {}",
258 car.vehicle.id,
259 start_dist,
260 car.router.get_end_dist(),
261 first_lane
262 );
263 params.router = car.router;
264 params.vehicle = car.vehicle;
265 return Some(params);
266 }
267 }
268
269 car.state = car.crossing_state(start_dist, now, ctx.map);
270 start_crossing = true;
271 }
272 ctx.scheduler
273 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
274 self.queues
275 .get_mut(&Traversable::Lane(first_lane))
276 .unwrap()
277 .insert_car_at_idx(idx, &car);
278 self.waiting_to_spawn.remove(&car.vehicle.id);
279
280 if start_crossing {
281 self.new_crossing_state(ctx, &car);
284 }
285
286 self.cars.insert(car.vehicle.id, car);
287
288 return None;
289 }
290 Some(params)
291 }
292
293 pub fn vehicle_waiting_to_spawn(&mut self, id: CarID, pos: Position, person: Option<PersonID>) {
296 self.waiting_to_spawn.insert(id, (pos, person));
297 }
298
299 pub fn update_car(
320 &mut self,
321 id: CarID,
322 now: Time,
323 ctx: &mut Ctx,
324 trips: &mut TripManager,
325 transit: &mut TransitSimState,
326 walking: &mut WalkingSimState,
327 ) {
328 let mut need_distances = {
329 let car = &self.cars[&id];
330 match car.state {
331 CarState::Queued { .. } => car.router.last_step(),
332 CarState::Parking(_, _, _) => true,
333 CarState::IdlingAtStop(_, _) => true,
334 _ => false,
335 }
336 };
337
338 if !need_distances {
339 let mut car = self.cars.remove(&id).unwrap();
342 need_distances = self.update_car_without_distances(&mut car, now, ctx, transit);
344 self.cars.insert(id, car);
345 }
346 if need_distances {
348 let dists = self.queues[&self.cars[&id].router.head()].get_car_positions(
350 now,
351 &self.cars,
352 &self.queues,
353 );
354 let idx = dists
355 .iter()
356 .position(|entry| entry.member == Queued::Vehicle(id))
357 .unwrap();
358
359 let mut car = self.cars.remove(&id).unwrap();
362 if self
364 .update_car_with_distances(&mut car, &dists, idx, now, ctx, trips, transit, walking)
365 {
366 self.cars.insert(id, car);
367 } else {
368 self.delete_car_internal(&mut car, dists, idx, now, ctx);
369 }
370 }
371 }
372
373 fn update_car_without_distances(
377 &mut self,
378 car: &mut Car,
379 now: Time,
380 ctx: &mut Ctx,
381 transit: &mut TransitSimState,
382 ) -> bool {
383 match car.state {
384 CarState::Crossing { .. } => {
385 car.state = CarState::Queued {
386 blocked_since: now,
387 want_to_change_lanes: None,
388 };
389 if car.router.last_step() {
390 return true;
392 }
393 let queue = &self.queues[&car.router.head()];
394 if queue.is_car_at_front(car.vehicle.id) {
395 car.state = CarState::WaitingToAdvance { blocked_since: now };
397 if self.recalc_lanechanging {
398 car.router.opportunistically_lanechange(
399 &self.queues,
400 ctx.map,
401 self.handle_uber_turns,
402 );
403 }
404 ctx.scheduler.push(now, Command::UpdateCar(car.vehicle.id));
405 } else if let Some(slow_leader) = self.wants_to_overtake(car) {
406 let first_conflict = car.wants_to_overtake.insert(slow_leader);
409
410 if first_conflict
412 && slow_leader.vehicle_type == VehicleType::Bike
413 && car.vehicle.vehicle_type != VehicleType::Bike
414 {
415 self.events.push(Event::ProblemEncountered(
416 self.cars[&slow_leader].trip_and_person.unwrap().0,
417 Problem::OvertakeDesired(queue.id),
418 ));
419 }
420
421 if let Some(target_lane) = self.pick_overtaking_lane(car, ctx.map) {
422 car.state = CarState::Queued {
426 blocked_since: now,
427 want_to_change_lanes: Some(target_lane),
428 };
429 return true;
430 }
431 }
432 }
433 CarState::Unparking {
434 front,
435 ref blocked_starts,
436 ..
437 } => {
438 for lane in blocked_starts {
439 self.cars.insert(car.vehicle.id, car.clone());
448
449 let dists = self.queues[&Traversable::Lane(*lane)].get_car_positions(
450 now,
451 &self.cars,
452 &self.queues,
453 );
454
455 self.cars.remove(&car.vehicle.id);
457
458 let idx = dists.iter().position(|entry| matches!(entry.member, Queued::StaticBlockage { cause, ..} if cause == car.vehicle.id)).unwrap();
459 self.update_follower(idx, &dists, now, ctx);
460
461 self.queues
462 .get_mut(&Traversable::Lane(*lane))
463 .unwrap()
464 .clear_static_blockage(car.vehicle.id, idx);
465 }
466
467 if car.router.last_step() {
468 car.router.maybe_handle_end(
473 front,
474 &car.vehicle,
475 ctx.parking,
476 ctx.map,
477 car.trip_and_person,
478 &mut self.events,
479 );
480 }
481 car.state = car.crossing_state(front, now, ctx.map);
482 ctx.scheduler
483 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
484 self.new_crossing_state(ctx, car);
485 }
486 CarState::WaitingToAdvance { blocked_since } => {
487 let from = car.router.head();
489 let goto = car.router.next();
490 assert!(from != goto);
491
492 if let Traversable::Turn(t) = goto {
493 if !ctx.intersections.maybe_start_turn(
494 AgentID::Car(car.vehicle.id),
495 t,
496 PathStep::Turn(t).max_speed_along(
497 car.vehicle.max_speed,
498 car.vehicle.vehicle_type.to_constraints(),
499 ctx.map,
500 ),
501 now,
502 ctx.map,
503 ctx.scheduler,
504 Some((car, &self.cars, &mut self.queues)),
505 ) {
506 return false;
508 }
509 if let Some((trip, _)) = car.trip_and_person {
510 self.events.push(Event::IntersectionDelayMeasured(
511 trip,
512 t,
513 AgentID::Car(car.vehicle.id),
514 now - blocked_since,
515 ));
516 }
517 }
518
519 {
520 let queue = self.queues.get_mut(&from).unwrap();
521 assert_eq!(queue.move_first_car_to_laggy_head(), car.vehicle.id);
522 }
523
524 let last_step = car.router.advance(
528 &car.vehicle,
529 ctx.parking,
530 ctx.map,
531 car.trip_and_person,
532 &mut self.events,
533 );
534 car.total_blocked_time += now - blocked_since;
535 car.state = car.crossing_state(Distance::ZERO, now, ctx.map);
536 ctx.scheduler
537 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
538 self.events.push(Event::AgentEntersTraversable(
539 AgentID::Car(car.vehicle.id),
540 car.trip_and_person.map(|(t, _)| t),
541 goto,
542 if car.vehicle.vehicle_type.is_transit() {
543 Some(transit.get_passengers(car.vehicle.id).len())
544 } else {
545 None
546 },
547 ));
548
549 car.last_steps.push_front(last_step);
551
552 ctx.scheduler.update(
556 car.crossing_state_with_end_dist(
557 DistanceInterval::new_driving(
558 Distance::ZERO,
559 car.vehicle.length + FOLLOWING_DISTANCE,
560 ),
561 now,
562 ctx.map,
563 )
564 .get_end_time(),
565 Command::UpdateLaggyHead(car.vehicle.id),
566 );
567
568 self.queues
569 .get_mut(&goto)
570 .unwrap()
571 .push_car_onto_end(car.vehicle.id);
572 }
573 CarState::ChangingLanes {
574 from,
575 new_time,
576 new_dist,
577 ..
578 } => {
579 car.state = CarState::Crossing {
582 time_int: new_time,
583 dist_int: new_dist,
584 steep_uphill: false,
585 };
586 ctx.scheduler
587 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
588
589 let dists = self.queues[&Traversable::Lane(from)].get_car_positions(
592 now,
593 &self.cars,
594 &self.queues,
595 );
596 let idx = dists.iter().position(|entry| matches!(entry.member, Queued::DynamicBlockage { cause, ..} if cause == car.vehicle.id)).unwrap();
597 self.update_follower(idx, &dists, now, ctx);
598
599 self.queues
600 .get_mut(&Traversable::Lane(from))
601 .unwrap()
602 .clear_dynamic_blockage(car.vehicle.id, idx);
603 }
604 CarState::Queued { .. } => unreachable!(),
605 CarState::Parking(_, _, _) => unreachable!(),
606 CarState::IdlingAtStop(_, _) => unreachable!(),
607 }
608 false
609 }
610
611 fn update_car_with_distances(
613 &mut self,
614 car: &mut Car,
615 dists: &[QueueEntry],
616 idx: usize,
617 now: Time,
618 ctx: &mut Ctx,
619 trips: &mut TripManager,
620 transit: &mut TransitSimState,
621 walking: &mut WalkingSimState,
622 ) -> bool {
623 let our_dist = dists[idx].front;
624
625 match car.state {
626 CarState::Crossing { .. }
627 | CarState::Unparking { .. }
628 | CarState::WaitingToAdvance { .. }
629 | CarState::ChangingLanes { .. } => unreachable!(),
630 CarState::Queued {
631 blocked_since,
632 want_to_change_lanes,
633 } => {
634 if let Some(target_lane) = want_to_change_lanes {
637 self.try_start_lc(car, our_dist, idx, target_lane, now, ctx);
638 return true;
639 }
640
641 match car.router.maybe_handle_end(
642 our_dist,
643 &car.vehicle,
644 ctx.parking,
645 ctx.map,
646 car.trip_and_person,
647 &mut self.events,
648 ) {
649 Some(ActionAtEnd::VanishAtBorder(i)) => {
650 car.total_blocked_time += now - blocked_since;
651 if car.trip_and_person.is_some() {
653 trips.car_or_bike_reached_border(
654 now,
655 car.vehicle.id,
656 i,
657 car.total_blocked_time,
658 car.router.get_path().total_length(),
659 ctx,
660 );
661 }
662 false
663 }
664 Some(ActionAtEnd::GiveUpOnParking) => {
665 car.total_blocked_time += now - blocked_since;
666 trips.cancel_trip(
667 now,
668 car.trip_and_person.unwrap().0,
669 "no available parking anywhere".to_string(),
670 None,
673 ctx,
674 );
675 false
676 }
677 Some(ActionAtEnd::StartParking(spot)) => {
678 car.total_blocked_time += now - blocked_since;
679 let delay = match spot {
680 ParkingSpot::Onstreet(_, _) => self.time_to_park_onstreet,
681 ParkingSpot::Offstreet(_, _) | ParkingSpot::Lot(_, _) => {
682 self.time_to_park_offstreet
683 }
684 };
685 car.state =
686 CarState::Parking(our_dist, spot, TimeInterval::new(now, now + delay));
687 ctx.parking.reserve_spot(spot, car.vehicle.id);
691 ctx.scheduler
692 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
693 true
694 }
695 Some(ActionAtEnd::GotoLaneEnd) => {
696 car.total_blocked_time += now - blocked_since;
697 car.state = car.crossing_state(our_dist, now, ctx.map);
698 ctx.scheduler
699 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
700 self.new_crossing_state(ctx, car);
701 true
702 }
703 Some(ActionAtEnd::StopBiking(bike_rack)) => {
704 car.total_blocked_time += now - blocked_since;
705 trips.bike_reached_end(
706 now,
707 car.vehicle.id,
708 bike_rack,
709 car.total_blocked_time,
710 car.router.get_path().total_length(),
711 ctx,
712 );
713 false
714 }
715 Some(ActionAtEnd::BusAtStop) => {
716 car.total_blocked_time += now - blocked_since;
717 if transit.bus_arrived_at_stop(now, car.vehicle.id, trips, walking, ctx) {
718 car.state = CarState::IdlingAtStop(
719 our_dist,
720 TimeInterval::new(now, now + TIME_TO_WAIT_AT_BUS_STOP),
721 );
722 ctx.scheduler
723 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
724 true
725 } else {
726 false
728 }
729 }
730 None => {
731 ctx.scheduler.push(
732 now + BLIND_RETRY_TO_REACH_END_DIST,
733 Command::UpdateCar(car.vehicle.id),
734 );
735
736 true
756 }
757 }
758 }
759 CarState::Parking(_, spot, _) => {
760 ctx.parking.add_parked_car(ParkedCar {
761 vehicle: car.vehicle.clone(),
762 spot,
763 parked_since: now,
764 });
765 trips.car_reached_parking_spot(
766 now,
767 car.vehicle.id,
768 spot,
769 car.total_blocked_time,
770 car.router.get_path().total_length(),
771 ctx,
772 );
773 false
774 }
775 CarState::IdlingAtStop(dist, _) => {
776 car.router = transit.bus_departed_from_stop(car.vehicle.id, ctx.map);
777 self.events
778 .push(Event::PathAmended(car.router.get_path().clone()));
779 car.state = car.crossing_state(dist, now, ctx.map);
780 ctx.scheduler
781 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
782 self.new_crossing_state(ctx, car);
783
784 self.update_follower(idx, dists, now, ctx);
785
786 true
787 }
788 }
789 }
790
791 pub fn delete_car(&mut self, c: CarID, now: Time, ctx: &mut Ctx) -> Vehicle {
794 self.waiting_to_spawn.remove(&c);
795
796 let dists = self.queues[&self.cars[&c].router.head()].get_car_positions(
797 now,
798 &self.cars,
799 &self.queues,
800 );
801 let idx = dists
802 .iter()
803 .position(|entry| entry.member == Queued::Vehicle(c))
804 .unwrap();
805 let mut car = self.cars.remove(&c).unwrap();
806
807 if let Traversable::Turn(t) = car.router.head() {
809 let queue = self.queues.get_mut(&car.router.head()).unwrap();
810 queue.reserved_length += car.vehicle.length + FOLLOWING_DISTANCE;
813 ctx.intersections.agent_deleted_mid_turn(AgentID::Car(c), t);
814
815 let queue = self.queues.get_mut(&car.router.next()).unwrap();
817 queue.free_reserved_space(&car);
818 }
819 if let Some(Traversable::Turn(t)) = car.router.maybe_next() {
820 ctx.intersections.cancel_request(AgentID::Car(c), t);
821 }
822
823 if car.router.last_step() {
824 ctx.parking.unreserve_spot(c);
825 }
826
827 self.delete_car_internal(&mut car, dists, idx, now, ctx);
828 ctx.scheduler.cancel(Command::UpdateCar(c));
830 car.vehicle
831 }
832
833 fn delete_car_internal(
834 &mut self,
835 car: &mut Car,
836 dists: Vec<QueueEntry>,
837 idx: usize,
838 now: Time,
839 ctx: &mut Ctx,
840 ) {
841 {
842 let queue = self.queues.get_mut(&car.router.head()).unwrap();
843 queue.remove_car_from_idx(car.vehicle.id, idx);
844 queue.free_reserved_space(car);
846 let i = match queue.id {
847 Traversable::Lane(l) => ctx.map.get_l(l).src_i,
848 Traversable::Turn(t) => t.parent,
849 };
850 if ctx.handling_live_edits.is_none() {
851 ctx.intersections
852 .space_freed(now, i, ctx.scheduler, ctx.map);
853 }
854 }
855
856 ctx.intersections.vehicle_gone(car.vehicle.id);
857
858 self.trim_last_steps(car, now, car.last_steps.len(), ctx);
860
861 ctx.scheduler
863 .cancel(Command::UpdateLaggyHead(car.vehicle.id));
864
865 self.update_follower(idx, &dists, now, ctx);
866 }
867
868 fn update_follower(
871 &mut self,
872 idx_leader: usize,
873 dists: &[QueueEntry],
874 now: Time,
875 ctx: &mut Ctx,
876 ) {
877 if idx_leader == dists.len() - 1 {
878 return;
879 }
880 let idx_follower = idx_leader + 1;
881
882 if let Queued::Vehicle(follower_id) = dists[idx_follower].member {
883 let follower_dist = dists[idx_follower].front;
884
885 if let Some(ref deleting_agents) = ctx.handling_live_edits {
887 if deleting_agents.contains(&AgentID::Car(follower_id)) {
888 return;
889 }
890 }
891
892 let follower = self.cars.get_mut(&follower_id).unwrap();
893 match follower.state {
897 CarState::Queued { blocked_since, .. } => {
898 follower.total_blocked_time += now - blocked_since;
903 follower.state = follower.crossing_state(follower_dist, now, ctx.map);
904 ctx.scheduler.update(
905 follower.state.get_end_time(),
906 Command::UpdateCar(follower_id),
907 );
908 let follower = &self.cars[&follower_id];
909 self.new_crossing_state(ctx, follower);
910 }
911 CarState::Crossing { .. } => {
912 follower.state = follower.crossing_state(follower_dist, now, ctx.map);
916 ctx.scheduler.update(
917 follower.state.get_end_time(),
918 Command::UpdateCar(follower_id),
919 );
920 let follower = &self.cars[&follower_id];
922 self.new_crossing_state(ctx, follower);
923 }
924 CarState::ChangingLanes {
925 from, to, lc_time, ..
926 } => {
927 let (new_time, new_dist) = match follower.crossing_state_with_end_dist(
935 DistanceInterval::new_driving(follower_dist, ctx.map.get_l(to).length()),
936 now,
937 ctx.map,
938 ) {
939 CarState::Crossing {
940 time_int, dist_int, ..
941 } => (time_int, dist_int),
942 _ => unreachable!(),
943 };
944 assert!(new_time.end >= lc_time.end);
945 follower.state = CarState::ChangingLanes {
946 from,
947 to,
948 new_time,
949 new_dist,
950 lc_time,
951 };
952 }
953 CarState::Unparking { .. }
955 | CarState::Parking(_, _, _)
956 | CarState::IdlingAtStop(_, _) => {}
957 CarState::WaitingToAdvance { .. } => unreachable!(),
958 }
959 }
960 }
961
962 pub fn update_laggy_head(&mut self, id: CarID, now: Time, ctx: &mut Ctx) {
963 let currently_on = self.cars[&id].router.head();
964 let dist_along_last = {
966 let (last_id, dist) = self.queues[¤tly_on]
967 .get_last_car_position(now, &self.cars, &self.queues)
968 .unwrap();
969 if id != last_id {
970 panic!(
971 "At {} on {:?}, laggy head {} isn't the last on the lane; it's {}",
972 now, currently_on, id, last_id
973 );
974 }
975 dist
976 };
977
978 let mut dist_left_to_cleanup = self.cars[&id].vehicle.length + FOLLOWING_DISTANCE;
980 dist_left_to_cleanup -= dist_along_last;
981 let mut num_to_trim = None;
982 for (idx, step) in self.cars[&id].last_steps.iter().enumerate() {
983 if dist_left_to_cleanup <= Distance::ZERO {
984 num_to_trim = Some(self.cars[&id].last_steps.len() - idx);
985 break;
986 }
987 dist_left_to_cleanup -= step.get_polyline(ctx.map).length();
988 }
989
990 if let Some(n) = num_to_trim {
991 let mut car = self.cars.remove(&id).unwrap();
992 self.trim_last_steps(&mut car, now, n, ctx);
993 self.cars.insert(id, car);
994 }
995
996 if !self.cars[&id].last_steps.is_empty() {
997 let retry_at = self.cars[&id]
999 .crossing_state_with_end_dist(
1000 DistanceInterval::new_driving(
1004 dist_along_last,
1005 self.cars[&id].vehicle.length + FOLLOWING_DISTANCE,
1006 ),
1007 now,
1008 ctx.map,
1009 )
1010 .get_end_time();
1011 if retry_at > now {
1015 ctx.scheduler.push(retry_at, Command::UpdateLaggyHead(id));
1016 } else {
1017 ctx.scheduler.push(
1022 now + BLIND_RETRY_TO_CREEP_FORWARDS,
1023 Command::UpdateLaggyHead(id),
1024 );
1025 }
1026 }
1027 }
1028
1029 fn trim_last_steps(&mut self, car: &mut Car, now: Time, n: usize, ctx: &mut Ctx) {
1031 for i in 0..n {
1032 let on = car.last_steps.pop_back().unwrap();
1033 let old_queue = self.queues.get_mut(&on).unwrap();
1034 assert_eq!(old_queue.laggy_head, Some(car.vehicle.id));
1035 old_queue.laggy_head = None;
1036 match on {
1037 Traversable::Turn(t) => {
1038 ctx.intersections.turn_finished(
1039 now,
1040 AgentID::Car(car.vehicle.id),
1041 t,
1042 ctx.scheduler,
1043 ctx.map,
1044 ctx.handling_live_edits.is_some(),
1045 );
1046 }
1047 Traversable::Lane(l) => {
1048 old_queue.free_reserved_space(car);
1049 if ctx.handling_live_edits.is_none() {
1050 ctx.intersections.space_freed(
1051 now,
1052 ctx.map.get_l(l).src_i,
1053 ctx.scheduler,
1054 ctx.map,
1055 );
1056 }
1057 }
1058 }
1059
1060 if i == 0 {
1061 if let Some(follower_id) = old_queue.get_active_cars().get(0) {
1063 if old_queue.is_car_at_front(*follower_id) {
1065 let follower = self.cars.get_mut(follower_id).unwrap();
1066
1067 match follower.state {
1068 CarState::Queued { blocked_since, .. } => {
1069 if !follower.router.last_step() {
1072 follower.state = CarState::WaitingToAdvance { blocked_since };
1076 if self.recalc_lanechanging && ctx.handling_live_edits.is_none()
1077 {
1078 follower.router.opportunistically_lanechange(
1079 &self.queues,
1080 ctx.map,
1081 self.handle_uber_turns,
1082 );
1083 }
1084 ctx.scheduler
1085 .push(now, Command::UpdateCar(follower.vehicle.id));
1086 }
1087 }
1088 CarState::WaitingToAdvance { .. } => unreachable!(),
1089 CarState::Crossing { .. }
1092 | CarState::ChangingLanes { .. }
1093 | CarState::Unparking { .. }
1094 | CarState::Parking(_, _, _)
1095 | CarState::IdlingAtStop(_, _) => {}
1096 }
1097 }
1098 }
1099 } else {
1100 assert!(old_queue.get_active_cars().is_empty());
1103 }
1104 }
1105 }
1106
1107 fn pick_overtaking_lane(&self, car: &Car, map: &Map) -> Option<LaneID> {
1113 let current_lane = map.get_l(car.router.head().maybe_lane()?);
1115 let road = map.get_parent(current_lane.id);
1116 let idx = current_lane.id.offset;
1117
1118 let mut candidates = Vec::new();
1119 if idx != 0 {
1120 candidates.push(road.lanes[idx - 1].id);
1121 }
1122 if idx != road.lanes.len() - 1 {
1123 candidates.push(road.lanes[idx + 1].id);
1124 }
1125 if map.get_config().driving_side == DrivingSide::Left {
1126 candidates.reverse();
1127 }
1128
1129 for l in candidates {
1130 let target_lane = map.get_l(l);
1131 if current_lane.dir != target_lane.dir {
1133 continue;
1134 }
1135 if !car
1138 .vehicle
1139 .vehicle_type
1140 .to_constraints()
1141 .can_use(target_lane, map)
1142 {
1143 continue;
1144 }
1145 if !car
1148 .router
1149 .can_lanechange(current_lane.id, target_lane.id, map)
1150 {
1151 continue;
1152 }
1153 return Some(target_lane.id);
1154 }
1155
1156 None
1157 }
1158
1159 fn try_start_lc(
1160 &mut self,
1161 car: &mut Car,
1162 front_current_queue: Distance,
1163 idx_in_current_queue: usize,
1164 target_lane: LaneID,
1165 now: Time,
1166 ctx: &mut Ctx,
1167 ) {
1168 if !car.last_steps.is_empty() {
1171 return;
1172 }
1173 if front_current_queue >= ctx.map.get_l(target_lane).length() {
1176 return;
1177 }
1178 let current_lane = car.router.head().as_lane();
1179 let front_target_queue = Position::new(current_lane, front_current_queue)
1180 .equiv_pos(target_lane, ctx.map)
1181 .dist_along();
1182
1183 let (new_time, new_dist) = match car.crossing_state_with_end_dist(
1186 DistanceInterval::new_driving(front_target_queue, ctx.map.get_l(target_lane).length()),
1187 now,
1188 ctx.map,
1189 ) {
1190 CarState::Crossing {
1191 time_int, dist_int, ..
1192 } => (time_int, dist_int),
1193 _ => unreachable!(),
1194 };
1195
1196 let lc_time = TimeInterval::new(now, now + TIME_TO_CHANGE_LANES);
1199 if lc_time.end >= new_time.end {
1200 return;
1201 }
1202
1203 if let Some(idx_in_target_queue) = self.queues[&Traversable::Lane(target_lane)]
1205 .get_idx_to_insert_car(
1206 front_target_queue,
1207 car.vehicle.length,
1208 now,
1209 &self.cars,
1210 &self.queues,
1211 )
1212 {
1213 if false {
1215 info!(
1216 "{} is starting to change lanes from {} to {}",
1217 car.vehicle.id,
1218 car.router.head(),
1219 target_lane
1220 );
1221 }
1222
1223 self.queues
1225 .get_mut(&car.router.head())
1226 .unwrap()
1227 .replace_car_with_dynamic_blockage(car, idx_in_current_queue);
1228
1229 car.router.confirm_lanechange(target_lane, ctx.map);
1231
1232 self.queues
1234 .get_mut(&car.router.head())
1235 .unwrap()
1236 .insert_car_at_idx(idx_in_target_queue, car);
1237
1238 car.state = CarState::ChangingLanes {
1240 from: current_lane,
1241 to: target_lane,
1242 new_time,
1243 new_dist,
1244 lc_time,
1245 };
1246 ctx.scheduler
1247 .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
1248 }
1249 }
1250
1251 pub fn collect_events(&mut self) -> Vec<Event> {
1252 std::mem::take(&mut self.events)
1253 }
1254
1255 pub fn handle_live_edits(&mut self, map: &Map) {
1256 let mut new_queues = HashSet::new();
1258 for l in map.all_lanes() {
1259 if l.lane_type.is_for_moving_vehicles() {
1260 new_queues.insert(Traversable::Lane(l.id));
1261 }
1262 }
1263 for t in map.all_turns() {
1264 if !t.between_sidewalks() {
1265 new_queues.insert(Traversable::Turn(t.id));
1266 }
1267 }
1268
1269 self.queues.retain(|k, v| {
1271 if new_queues.remove(k) {
1272 true
1274 } else {
1275 if v.laggy_head.is_some() || !v.get_active_cars().is_empty() {
1277 panic!(
1278 "After live map edits, deleted queue {} still has vehicles! {:?}, {:?}",
1279 k,
1280 v.laggy_head,
1281 v.get_active_cars()
1282 );
1283 }
1284 false
1285 }
1286 });
1287
1288 for key in new_queues {
1290 self.queues.insert(key, Queue::new(key, map));
1291 }
1292 }
1293
1294 fn new_crossing_state(&self, ctx: &mut Ctx, car: &Car) {
1295 if self.queues[&car.router.head()].is_car_at_front(car.vehicle.id) {
1296 if let Some(Traversable::Turn(turn)) = car.router.maybe_next() {
1297 ctx.intersections.approaching_leader(
1298 AgentID::Car(car.vehicle.id),
1299 turn,
1300 car.state.get_end_time(),
1301 );
1302 }
1303 }
1304 }
1305}
1306
1307impl DrivingSimState {
1309 pub fn get_unzoomed_agents(&self, now: Time, map: &Map) -> Vec<UnzoomedAgent> {
1311 let mut result = Vec::new();
1312
1313 for queue in self.queues.values() {
1314 if queue.get_active_cars().is_empty() {
1315 continue;
1316 }
1317
1318 for entry in queue.get_car_positions(now, &self.cars, &self.queues) {
1319 if let Queued::Vehicle(c) = entry.member {
1320 let car = &self.cars[&c];
1321 result.push(UnzoomedAgent {
1322 id: AgentID::Car(car.vehicle.id),
1323 pos: match queue.id.get_polyline(map).dist_along(entry.front) {
1324 Ok((pt, _)) => pt,
1325 Err(err) => panic!(
1326 "At {}, invalid dist_along of {} for queue {}: {}",
1327 now, entry.front, queue.id, err
1328 ),
1329 },
1330 person: car.trip_and_person.map(|(_, p)| p),
1331 parking: car.is_parking(),
1332 });
1333 }
1334 }
1335 }
1336
1337 for (id, (pos, person)) in &self.waiting_to_spawn {
1338 result.push(UnzoomedAgent {
1339 id: AgentID::Car(*id),
1340 pos: pos.pt(map),
1341 person: *person,
1342 parking: false,
1343 });
1344 }
1345
1346 result
1347 }
1348
1349 pub fn does_car_exist(&self, id: CarID) -> bool {
1350 match self.cars.get(&id) {
1353 Some(car) => car.vehicle.id == id,
1354 None => false,
1355 }
1356 }
1357
1358 pub fn get_all_draw_cars(
1360 &self,
1361 now: Time,
1362 map: &Map,
1363 transit: &TransitSimState,
1364 ) -> Vec<DrawCarInput> {
1365 let mut result = Vec::new();
1366 for queue in self.queues.values() {
1367 result.extend(
1368 queue
1369 .get_car_positions(now, &self.cars, &self.queues)
1370 .into_iter()
1371 .filter_map(|entry| {
1372 if let Queued::Vehicle(id) = entry.member {
1373 Some(self.cars[&id].get_draw_car(entry.front, now, map, transit))
1374 } else {
1375 None
1376 }
1377 }),
1378 );
1379 }
1380 result
1381 }
1382
1383 pub fn get_single_draw_car(
1385 &self,
1386 id: CarID,
1387 now: Time,
1388 map: &Map,
1389 transit: &TransitSimState,
1390 ) -> Option<DrawCarInput> {
1391 let car = self.cars.get(&id)?;
1392 self.get_draw_cars_on(now, car.router.head(), map, transit)
1393 .into_iter()
1394 .find(|d| d.id == id)
1395 }
1396
1397 pub fn get_draw_cars_on(
1398 &self,
1399 now: Time,
1400 on: Traversable,
1401 map: &Map,
1402 transit: &TransitSimState,
1403 ) -> Vec<DrawCarInput> {
1404 match self.queues.get(&on) {
1405 Some(q) => q
1406 .get_car_positions(now, &self.cars, &self.queues)
1407 .into_iter()
1408 .filter_map(|entry| match entry.member {
1409 Queued::Vehicle(id) => {
1410 Some(self.cars[&id].get_draw_car(entry.front, now, map, transit))
1411 }
1412 Queued::StaticBlockage { cause, front, back } => {
1414 if false {
1415 Some(DrawCarInput {
1416 id: cause,
1417 waiting_for_turn: None,
1418 status: CarStatus::Parked,
1419 intent: None,
1420 on,
1421 partly_on: Vec::new(),
1422 label: Some("block".to_string()),
1423 person: None,
1424 body: on.get_polyline(map).exact_slice(back, front),
1425 })
1426 } else {
1427 None
1428 }
1429 }
1430 Queued::DynamicBlockage { cause, vehicle_len } => {
1431 if false {
1432 Some(DrawCarInput {
1433 id: cause,
1434 waiting_for_turn: None,
1435 status: CarStatus::Parked,
1436 intent: None,
1437 on,
1438 partly_on: Vec::new(),
1439 label: Some("block".to_string()),
1440 person: None,
1441 body: on
1442 .get_polyline(map)
1443 .exact_slice(entry.front - vehicle_len, entry.front),
1444 })
1445 } else {
1446 None
1447 }
1448 }
1449 })
1450 .collect(),
1451 None => Vec::new(),
1452 }
1453 }
1454
1455 pub fn debug_car_json(&self, id: CarID) -> String {
1456 if let Some(ref car) = self.cars.get(&id) {
1457 abstutil::to_json(car)
1458 } else {
1459 format!("{} is parked somewhere", id)
1460 }
1461 }
1462
1463 pub fn debug_car_ui(&self, id: CarID) -> String {
1464 if let Some(car) = self.cars.get(&id) {
1465 format!("{:?}", car.state)
1466 } else {
1467 format!("{} isn't in DrivingSimState", id)
1468 }
1469 }
1470
1471 pub fn debug_lane(&self, id: LaneID) {
1472 if let Some(ref queue) = self.queues.get(&Traversable::Lane(id)) {
1473 println!("{}", abstutil::to_json(queue));
1474 }
1475 }
1476
1477 pub fn agent_properties(&self, id: CarID, now: Time) -> AgentProperties {
1478 if let Some(car) = self.cars.get(&id) {
1479 let path = car.router.get_path();
1480 let time_spent_waiting = car.state.time_spent_waiting(now);
1481
1482 let front = self.get_car_front(now, car);
1485 let current_state_dist =
1486 if car.router.head() == Traversable::Lane(path.get_req().start.lane()) {
1487 front - path.get_req().start.dist_along()
1488 } else {
1489 front
1490 };
1491
1492 AgentProperties {
1493 total_time: now - car.started_at,
1494 waiting_here: time_spent_waiting,
1495 total_waiting: car.total_blocked_time + time_spent_waiting,
1496 dist_crossed: path.crossed_so_far() + current_state_dist,
1497 total_dist: path.total_length(),
1498 }
1499 } else {
1500 for car in self.waiting_to_spawn.keys() {
1501 if id == *car {
1502 return AgentProperties {
1506 total_time: Duration::ZERO,
1507 waiting_here: Duration::ZERO,
1508 total_waiting: Duration::ZERO,
1509 dist_crossed: Distance::ZERO,
1510 total_dist: Distance::ZERO,
1511 };
1512 }
1513 }
1514 panic!(
1515 "Can't get agent_properties of {} at {}; they don't exist",
1516 id, now
1517 );
1518 }
1519 }
1520
1521 pub fn get_path(&self, id: CarID) -> Option<&Path> {
1522 let car = self.cars.get(&id)?;
1523 Some(car.router.get_path())
1524 }
1525 pub fn get_all_driving_paths(&self) -> Vec<&Path> {
1526 self.cars
1527 .values()
1528 .map(|car| car.router.get_path())
1529 .collect()
1530 }
1531
1532 pub fn trace_route(&self, now: Time, id: CarID, map: &Map) -> Option<PolyLine> {
1533 let car = self.cars.get(&id)?;
1534 let front = self.get_car_front(now, car);
1535 car.router.get_path().trace_from_start(map, front)
1536 }
1537
1538 pub fn percent_along_route(&self, id: CarID) -> f64 {
1539 self.cars[&id].router.get_path().percent_dist_crossed()
1540 }
1541
1542 pub fn get_owner_of_car(&self, id: CarID) -> Option<PersonID> {
1543 let car = self.cars.get(&id)?;
1544 car.vehicle.owner
1545 }
1546
1547 pub fn target_lane_penalty(&self, l: LaneID) -> (usize, usize) {
1548 self.queues[&Traversable::Lane(l)].target_lane_penalty()
1549 }
1550
1551 pub fn find_trips_to_edited_parking(
1552 &self,
1553 spots: BTreeSet<ParkingSpot>,
1554 ) -> Vec<(AgentID, TripID)> {
1555 let mut affected = Vec::new();
1556 for car in self.cars.values() {
1557 if let Some(spot) = car.router.get_parking_spot_goal() {
1558 if !spots.contains(spot) {
1559 affected.push((AgentID::Car(car.vehicle.id), car.trip_and_person.unwrap().0));
1561 }
1562 }
1563 }
1564 affected
1565 }
1566
1567 pub fn find_vehicles_affected_by_live_edits(
1569 &self,
1570 closed_intersections: &HashSet<IntersectionID>,
1571 edited_lanes: &BTreeSet<LaneID>,
1572 ) -> Vec<(AgentID, TripID)> {
1573 let mut affected = Vec::new();
1574 for car in self.cars.values() {
1575 if car.last_steps.iter().any(|step| match step {
1576 Traversable::Lane(l) => edited_lanes.contains(l),
1577 Traversable::Turn(t) => {
1578 closed_intersections.contains(&t.parent)
1579 || edited_lanes.contains(&t.src)
1580 || edited_lanes.contains(&t.dst)
1581 }
1582 }) {
1583 if let Some((trip, _)) = car.trip_and_person {
1586 affected.push((AgentID::Car(car.vehicle.id), trip));
1587 }
1588 }
1589 }
1590 affected
1591 }
1592
1593 pub fn all_waiting_people(&self, now: Time, delays: &mut BTreeMap<PersonID, Duration>) {
1594 for c in self.cars.values() {
1595 if let Some((_, person)) = c.trip_and_person {
1596 let delay = c.state.time_spent_waiting(now);
1597 if delay > Duration::ZERO {
1598 delays.insert(person, delay);
1599 }
1600 }
1601 }
1602 }
1603
1604 pub fn debug_queue_lengths(&self, l: LaneID) -> Option<(Distance, Distance)> {
1605 let queue = self.queues.get(&Traversable::Lane(l))?;
1606 Some((queue.reserved_length, queue.geom_len))
1607 }
1608
1609 pub fn get_blocked_by_graph(
1610 &self,
1611 now: Time,
1612 map: &Map,
1613 intersections: &IntersectionSimState,
1614 ) -> BTreeMap<AgentID, (Duration, DelayCause)> {
1615 let mut graph = BTreeMap::new();
1616
1617 for queue in self.queues.values() {
1620 if let Some(head) = queue.laggy_head {
1621 if let Some(next) = queue.get_active_cars().get(0) {
1622 graph.insert(
1623 AgentID::Car(*next),
1624 (
1625 self.cars[&head].state.time_spent_waiting(now),
1626 DelayCause::Agent(AgentID::Car(head)),
1627 ),
1628 );
1629 }
1630 }
1631 let cars = queue.get_active_cars();
1634 for (head, tail) in cars.iter().zip(cars.iter().skip(1)) {
1635 graph.insert(
1636 AgentID::Car(*tail),
1637 (
1638 self.cars[tail].state.time_spent_waiting(now),
1639 DelayCause::Agent(AgentID::Car(*head)),
1640 ),
1641 );
1642 }
1643 }
1644
1645 intersections.populate_blocked_by(now, &mut graph, map, &self.cars, &self.queues);
1646 graph
1647 }
1648
1649 fn get_car_front(&self, now: Time, car: &Car) -> Distance {
1650 self.queues[&car.router.head()]
1651 .get_car_positions(now, &self.cars, &self.queues)
1652 .into_iter()
1653 .find(|entry| entry.member == Queued::Vehicle(car.vehicle.id))
1654 .unwrap()
1655 .front
1656 }
1657
1658 fn wants_to_overtake(&self, car: &Car) -> Option<CarID> {
1660 let queue = &self.queues[&car.router.head()];
1661 let leader = &self.cars[&queue.get_leader(car.vehicle.id)?];
1662
1663 let their_speed = leader.vehicle.max_speed?;
1667 if car
1668 .vehicle
1669 .max_speed
1670 .map(|s| s <= their_speed)
1671 .unwrap_or(false)
1672 {
1673 return None;
1674 }
1675
1676 match leader.state {
1678 CarState::WaitingToAdvance { .. } | CarState::Queued { .. } => {
1681 return None;
1682 }
1683 _ => {}
1685 }
1686
1687 Some(leader.vehicle.id)
1694 }
1695}
1696
1697impl IndexableKey for CarID {
1700 fn index(&self) -> usize {
1701 self.id
1702 }
1703}