paiagram_core/trip/
routing.rs

1use bevy::prelude::*;
2use itertools::Itertools;
3
4use crate::{
5    entry::{DerivedEntryBundle, EntryEstimate, EntryMode, EntryStop, IsDerivedEntry, TravelMode},
6    graph::Graph,
7    interval::IntervalQuery,
8    station::ParentStationOrStation,
9    trip::{TripClass, TripNominalSchedule, TripQuery, TripSchedule},
10    units::{
11        distance::Distance,
12        time::{Duration, TimetableTime},
13    },
14};
15
16pub struct RoutingPlugin;
17
18impl Plugin for RoutingPlugin {
19    fn build(&self, app: &mut App) {
20        app.add_message::<AddEntryToTrip>().add_systems(
21            Update,
22            (add_entries, recalculate_route, recalculate_estimate).chain(),
23        );
24    }
25}
26
27#[derive(Message, Clone, Copy)]
28pub struct AddEntryToTrip {
29    pub trip: Entity,
30    pub entry: Entity,
31}
32
33pub fn add_entries(
34    mut msgs: MessageReader<AddEntryToTrip>,
35    mut commands: Commands,
36    mut trips: Query<&mut TripNominalSchedule>,
37) {
38    for AddEntryToTrip { trip, entry } in msgs.read().copied() {
39        commands.entity(trip).add_child(entry);
40        trips.get_mut(trip).unwrap().push(entry);
41    }
42}
43
44pub fn recalculate_route(
45    changed_schedule: Query<
46        (Entity, &TripNominalSchedule, &mut TripSchedule),
47        Changed<TripNominalSchedule>,
48    >,
49    entry_q: Query<(Entity, &EntryStop)>,
50    derived_q: Query<Entity, With<IsDerivedEntry>>,
51    graph: Res<Graph>,
52    parent_station_or_station: Query<ParentStationOrStation>,
53    mut commands: Commands,
54    interval_q: Query<IntervalQuery>,
55) {
56    for (trip_entity, nominal_schedule, mut actual_schedule) in changed_schedule {
57        for derived_entity in derived_q.iter_many(actual_schedule.iter()) {
58            commands.entity(derived_entity).despawn();
59        }
60        recalculate_inner(
61            nominal_schedule,
62            trip_entity,
63            &mut actual_schedule,
64            &graph,
65            &mut commands,
66            &parent_station_or_station,
67            &entry_q,
68            &interval_q,
69        );
70    }
71}
72
73fn recalculate_inner(
74    route: &[Entity],
75    trip_entity: Entity,
76    buffer: &mut Vec<Entity>,
77    graph: &Graph,
78    commands: &mut Commands,
79    parent_station_or_station: &Query<ParentStationOrStation>,
80    entry_q: &Query<(Entity, &EntryStop)>,
81    interval_q: &Query<IntervalQuery>,
82) {
83    buffer.clear();
84    let mut route_iter = entry_q.iter_many(route.iter()).map(|(a, c)| {
85        (
86            a,
87            parent_station_or_station.get(c.entity()).unwrap().parent(),
88        )
89    });
90    let Some((prev_entity, mut prev_stop)) = route_iter.next() else {
91        return;
92    };
93    buffer.push(prev_entity);
94    for (curr_entity, stops) in route_iter.map(|(curr_entity, curr_stop)| {
95        if prev_stop == curr_stop || graph.contains_edge(prev_stop.entity(), curr_stop.entity()) {
96            prev_stop = curr_stop;
97            return (curr_entity, Vec::new());
98        }
99        let Some((_, mut station_list)) =
100            graph.route_between(prev_stop.entity(), curr_stop.entity(), interval_q)
101        else {
102            prev_stop = curr_stop;
103            return (curr_entity, Vec::new());
104        };
105        prev_stop = curr_stop;
106        station_list.remove(0);
107        station_list.pop();
108        return (curr_entity, station_list);
109    }) {
110        for stop in stops {
111            let e = commands.spawn(DerivedEntryBundle::new(stop)).id();
112            commands.entity(trip_entity).add_child(e);
113            buffer.push(e)
114        }
115        buffer.push(curr_entity);
116    }
117}
118
119/// Parameters used for unwinding the flexible stack.
120enum UnwindParams {
121    At(TimetableTime),
122    ForAt(Duration, TimetableTime),
123    ForFor(Duration, Duration),
124}
125
126/// Recalculate the estimates for updated routes.
127/// This should always run after [`recalculate_route`].
128fn recalculate_estimate(
129    changed_trips: Query<Entity, (Changed<TripSchedule>, With<TripClass>)>,
130    changed_entries: Query<&ChildOf, Changed<EntryMode>>,
131    trip_q: Query<TripQuery>,
132    entry_q: Query<(Entity, &EntryMode, &EntryStop)>,
133    parent_station_or_station: Query<ParentStationOrStation>,
134    interval_q: Query<IntervalQuery>,
135    mut commands: Commands,
136    graph: Res<Graph>,
137) {
138    let mut to_recalculate = changed_entries
139        .iter()
140        .map(|c| c.parent())
141        .chain(changed_trips.iter())
142        .collect::<Vec<_>>();
143    to_recalculate.sort_unstable();
144    to_recalculate.dedup();
145    for q in trip_q.iter_many(to_recalculate) {
146        let mut flexible_stack: Vec<(Entity, Entity, Duration)> = Vec::new();
147        let mut last_stable: Option<(TimetableTime, Entity)> = None;
148        let mut next_stable: Option<(TimetableTime, Entity)> = None;
149        let mut unwind_params: Option<UnwindParams> = None;
150        'iter_entries: for (entry_entity, mode, stop) in entry_q.iter_many(q.schedule.iter()) {
151            if let Some(v) = next_stable.take() {
152                last_stable = Some(v);
153            }
154            match (mode.arr.unwrap_or(TravelMode::Flexible), mode.dep) {
155                (TravelMode::At(at), TravelMode::At(dt)) => {
156                    commands
157                        .entity(entry_entity)
158                        .insert(EntryEstimate::new(at, dt));
159                    next_stable = Some((dt, stop.entity()));
160                    unwind_params = Some(UnwindParams::At(at));
161                }
162                (TravelMode::At(at), TravelMode::For(dd)) => {
163                    commands
164                        .entity(entry_entity)
165                        .insert(EntryEstimate::new(at, at + dd));
166                    next_stable = Some((at + dd, stop.entity()));
167                    unwind_params = Some(UnwindParams::At(at));
168                }
169                (TravelMode::At(at), TravelMode::Flexible) => {
170                    commands
171                        .entity(entry_entity)
172                        .insert(EntryEstimate::new(at, at));
173                    next_stable = Some((at, stop.entity()));
174                    unwind_params = Some(UnwindParams::At(at));
175                }
176                (TravelMode::For(ad), TravelMode::At(dt)) => {
177                    // estimates are inserted afterwards
178                    next_stable = Some((dt, stop.entity()));
179                    unwind_params = Some(UnwindParams::ForAt(ad, dt));
180                }
181                (TravelMode::For(ad), TravelMode::For(dd)) => {
182                    // estimates are inserted afterwards
183                    unwind_params = Some(UnwindParams::ForFor(ad, dd));
184                }
185                (TravelMode::For(ad), TravelMode::Flexible) => {
186                    // estimates are inserted afterwards
187                    unwind_params = Some(UnwindParams::ForFor(ad, Duration::ZERO));
188                }
189                (TravelMode::Flexible, TravelMode::At(dt)) => {
190                    commands
191                        .entity(entry_entity)
192                        .insert(EntryEstimate::new(dt, dt));
193                    next_stable = Some((dt, stop.entity()));
194                    unwind_params = Some(UnwindParams::At(dt))
195                }
196                (TravelMode::Flexible, TravelMode::For(dd)) => {
197                    flexible_stack.push((entry_entity, stop.entity(), dd))
198                }
199                (TravelMode::Flexible, TravelMode::Flexible) => {
200                    flexible_stack.push((entry_entity, stop.entity(), Duration::ZERO))
201                }
202            }
203            let Some(params) = unwind_params.take() else {
204                continue;
205            };
206            let Some((last_t, last_s)) = last_stable else {
207                for (e, _, _) in flexible_stack.drain(..) {
208                    commands.entity(e).remove::<EntryEstimate>();
209                }
210                continue;
211            };
212            let initial_t = last_t;
213            let total_stop_dur: Duration = flexible_stack.iter().map(|(_, _, d)| *d).sum();
214            let total_dur = match params {
215                UnwindParams::ForAt(d, _t) => d,
216                UnwindParams::ForFor(ad, _dd) => ad,
217                UnwindParams::At(t) => t - initial_t,
218            };
219            // stopping time should not be counted while average velocity
220            let travel_dur = total_dur - total_stop_dur;
221            let mut distance_stack = Vec::with_capacity(flexible_stack.len());
222
223            for (ps, cs) in std::iter::once(last_s)
224                .chain(flexible_stack.iter().map(|(_, s, _)| *s))
225                .chain(std::iter::once(stop.entity()))
226                .map(|e| parent_station_or_station.get(e).unwrap().parent())
227                .tuple_windows()
228            {
229                let Some(weight) = graph
230                    .edge_weight(ps, cs)
231                    .copied()
232                    .map(|w| interval_q.get(w).ok())
233                    .flatten()
234                else {
235                    for (e, _, _) in flexible_stack.drain(..) {
236                        commands.entity(e).remove::<EntryEstimate>();
237                    }
238                    continue 'iter_entries;
239                };
240                distance_stack.push(weight.distance())
241            }
242            debug_assert_eq!(distance_stack.len(), flexible_stack.len() + 1);
243            let total_dis = distance_stack.iter().cloned().sum::<Distance>();
244            let mut fi = flexible_stack.drain(..);
245            let mut di = distance_stack.drain(..);
246            let total_dis_m = total_dis.0 as f64;
247            let travel_dur_s = travel_dur.0 as f64;
248            let mut last_t_f = last_t.0 as f64;
249            while let (Some((e, _, dur)), Some(dis)) = (fi.next(), di.next()) {
250                let dis_m = dis.0 as f64;
251                let travel_leg_s = if total_dis_m == 0.0 {
252                    0.0
253                } else {
254                    travel_dur_s * (dis_m / total_dis_m)
255                };
256                last_t_f += travel_leg_s;
257                let arr = TimetableTime(last_t_f.round() as i32);
258                commands.entity(e).insert(EntryEstimate {
259                    arr,
260                    dep: arr + dur,
261                });
262                last_t_f += dur.0 as f64;
263            }
264            match params {
265                UnwindParams::At(_) => {}
266                UnwindParams::ForAt(d, t) => {
267                    commands.entity(entry_entity).insert(EntryEstimate {
268                        arr: initial_t + d,
269                        dep: t,
270                    });
271                }
272                UnwindParams::ForFor(ad, dd) => {
273                    commands.entity(entry_entity).insert(EntryEstimate {
274                        arr: initial_t + ad,
275                        dep: initial_t + ad + dd,
276                    });
277                    next_stable = Some((initial_t + ad + dd, stop.entity()))
278                }
279            }
280        }
281        for (e, _, _) in flexible_stack {
282            commands.entity(e).remove::<EntryEstimate>();
283        }
284    }
285}
paiagram_core/trip/routing.rs

paiagram_core/trip/
routing.rs
