paiagram_core/trip/
routing.rs

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

paiagram_core/trip/
routing.rs
