solverforge_maps/routing/
matrix.rs

1//! Matrix computation for road networks.
2
3use rayon::prelude::*;
4use std::collections::HashMap;
5use tokio::sync::mpsc::Sender;
6
7use super::algo::dijkstra;
8use super::coord::Coord;
9use super::graph::NodeIdx;
10use super::network::{EdgeSnappedLocation, RoadNetwork, SnappedCoord};
11use super::progress::RoutingProgress;
12
13pub const UNREACHABLE: i64 = i64::MAX;
14
15#[derive(Debug, Clone, Default)]
16pub struct TravelTimeMatrix {
17    data: Vec<i64>,
18    size: usize,
19    locations: Vec<SnappedCoord>,
20}
21
22impl TravelTimeMatrix {
23    pub(crate) fn new(data: Vec<i64>, size: usize, locations: Vec<SnappedCoord>) -> Self {
24        debug_assert_eq!(data.len(), size * size);
25        debug_assert_eq!(locations.len(), size);
26        Self {
27            data,
28            size,
29            locations,
30        }
31    }
32
33    /// Get the travel time from one location to another.
34    ///
35    /// Returns `None` if indices are out of bounds.
36    /// Returns `Some(UNREACHABLE)` if the pair is not reachable.
37    #[inline]
38    pub fn get(&self, from: usize, to: usize) -> Option<i64> {
39        if from < self.size && to < self.size {
40            Some(self.data[from * self.size + to])
41        } else {
42            None
43        }
44    }
45
46    #[inline]
47    pub fn is_reachable(&self, from: usize, to: usize) -> bool {
48        self.get(from, to)
49            .map(|v| v != UNREACHABLE)
50            .unwrap_or(false)
51    }
52
53    #[inline]
54    pub fn size(&self) -> usize {
55        self.size
56    }
57
58    #[inline]
59    pub fn locations(&self) -> &[SnappedCoord] {
60        &self.locations
61    }
62
63    #[inline]
64    pub fn row(&self, i: usize) -> Option<&[i64]> {
65        if i < self.size {
66            let start = i * self.size;
67            Some(&self.data[start..start + self.size])
68        } else {
69            None
70        }
71    }
72
73    pub fn min(&self) -> Option<i64> {
74        let mut min_val = None;
75        for i in 0..self.size {
76            for j in 0..self.size {
77                if i == j {
78                    continue;
79                }
80                let val = self.data[i * self.size + j];
81                if val != UNREACHABLE {
82                    min_val = Some(min_val.map(|m: i64| m.min(val)).unwrap_or(val));
83                }
84            }
85        }
86        min_val
87    }
88
89    pub fn max(&self) -> Option<i64> {
90        let mut max_val = None;
91        for i in 0..self.size {
92            for j in 0..self.size {
93                if i == j {
94                    continue;
95                }
96                let val = self.data[i * self.size + j];
97                if val != UNREACHABLE {
98                    max_val = Some(max_val.map(|m: i64| m.max(val)).unwrap_or(val));
99                }
100            }
101        }
102        max_val
103    }
104
105    pub fn mean(&self) -> Option<f64> {
106        let mut sum = 0i64;
107        let mut count = 0usize;
108        for i in 0..self.size {
109            for j in 0..self.size {
110                if i == j {
111                    continue;
112                }
113                let val = self.data[i * self.size + j];
114                if val != UNREACHABLE {
115                    sum = sum.saturating_add(val);
116                    count += 1;
117                }
118            }
119        }
120        if count > 0 {
121            Some(sum as f64 / count as f64)
122        } else {
123            None
124        }
125    }
126
127    pub fn reachability_ratio(&self) -> f64 {
128        if self.size <= 1 {
129            return 1.0;
130        }
131        let total_pairs = self.size * (self.size - 1);
132        let reachable = self.count_reachable();
133        reachable as f64 / total_pairs as f64
134    }
135
136    fn count_reachable(&self) -> usize {
137        let mut count = 0;
138        for i in 0..self.size {
139            for j in 0..self.size {
140                if i != j && self.data[i * self.size + j] != UNREACHABLE {
141                    count += 1;
142                }
143            }
144        }
145        count
146    }
147
148    pub fn unreachable_pairs(&self) -> Vec<(usize, usize)> {
149        let mut pairs = Vec::new();
150        for i in 0..self.size {
151            for j in 0..self.size {
152                if i != j && self.data[i * self.size + j] == UNREACHABLE {
153                    pairs.push((i, j));
154                }
155            }
156        }
157        pairs
158    }
159
160    pub fn as_slice(&self) -> &[i64] {
161        &self.data
162    }
163}
164
165impl RoadNetwork {
166    pub async fn compute_matrix(
167        &self,
168        locations: &[Coord],
169        progress: Option<&Sender<RoutingProgress>>,
170    ) -> TravelTimeMatrix {
171        let n = locations.len();
172        if n == 0 {
173            return TravelTimeMatrix::new(vec![], 0, vec![]);
174        }
175
176        let edge_snapped: Vec<Option<EdgeSnappedLocation>> = locations
177            .iter()
178            .map(|&coord| self.snap_to_edge(coord).ok())
179            .collect();
180
181        let snapped: Vec<SnappedCoord> = locations
182            .iter()
183            .zip(edge_snapped.iter())
184            .map(|(&coord, edge_snap)| {
185                if let Some(es) = edge_snap {
186                    SnappedCoord {
187                        original: coord,
188                        snapped: es.snapped,
189                        snap_distance_m: es.snap_distance_m,
190                        node_index: es.from_node,
191                    }
192                } else {
193                    SnappedCoord {
194                        original: coord,
195                        snapped: coord,
196                        snap_distance_m: f64::INFINITY,
197                        node_index: NodeIdx::new(0),
198                    }
199                }
200            })
201            .collect();
202
203        // Compute rows in parallel via rayon - each row runs Dijkstra from source endpoints
204        let graph = &self.graph;
205        let rows: Vec<Vec<i64>> = (0..n)
206            .into_par_iter()
207            .map(|i| {
208                let mut row = vec![0i64; n];
209
210                let Some(from) = &edge_snapped[i] else {
211                    for (j, cell) in row.iter_mut().enumerate() {
212                        if i != j {
213                            *cell = UNREACHABLE;
214                        }
215                    }
216                    return row;
217                };
218
219                let from_edge = match graph.edge_weight(from.edge_index) {
220                    Some(e) => e,
221                    None => {
222                        for (j, cell) in row.iter_mut().enumerate() {
223                            if i != j {
224                                *cell = UNREACHABLE;
225                            }
226                        }
227                        return row;
228                    }
229                };
230
231                // Run Dijkstra from both endpoints of source edge to all nodes
232                let costs_a = dijkstra(graph, from.from_node, None, |e| e.travel_time_s);
233                let costs_b = dijkstra(graph, from.to_node, None, |e| e.travel_time_s);
234
235                // Offset from snap point to each endpoint
236                let off_a = from_edge.travel_time_s * from.position;
237                let off_b = from_edge.travel_time_s * (1.0 - from.position);
238
239                for j in 0..n {
240                    if i == j {
241                        continue;
242                    }
243
244                    let Some(to) = &edge_snapped[j] else {
245                        row[j] = UNREACHABLE;
246                        continue;
247                    };
248
249                    let to_edge = match graph.edge_weight(to.edge_index) {
250                        Some(e) => e,
251                        None => {
252                            row[j] = UNREACHABLE;
253                            continue;
254                        }
255                    };
256
257                    let to_off_a = to_edge.travel_time_s * to.position;
258                    let to_off_b = to_edge.travel_time_s * (1.0 - to.position);
259
260                    // Best of 4 combinations: (from endpoint) -> (to endpoint)
261                    let mut best = f64::MAX;
262                    if let Some(&c) = costs_a.get(&to.from_node) {
263                        best = best.min(off_a + c + to_off_a);
264                    }
265                    if let Some(&c) = costs_a.get(&to.to_node) {
266                        best = best.min(off_a + c + to_off_b);
267                    }
268                    if let Some(&c) = costs_b.get(&to.from_node) {
269                        best = best.min(off_b + c + to_off_a);
270                    }
271                    if let Some(&c) = costs_b.get(&to.to_node) {
272                        best = best.min(off_b + c + to_off_b);
273                    }
274
275                    row[j] = if best == f64::MAX {
276                        UNREACHABLE
277                    } else {
278                        best.round() as i64
279                    };
280                }
281
282                row
283            })
284            .collect();
285
286        if let Some(tx) = progress {
287            let _ = tx
288                .send(RoutingProgress::ComputingMatrix {
289                    percent: 80,
290                    row: n,
291                    total: n,
292                })
293                .await;
294        }
295
296        let data: Vec<i64> = rows.into_iter().flatten().collect();
297        TravelTimeMatrix::new(data, n, snapped)
298    }
299
300    pub async fn compute_geometries(
301        &self,
302        locations: &[Coord],
303        progress: Option<&Sender<RoutingProgress>>,
304    ) -> HashMap<(usize, usize), Vec<Coord>> {
305        let n = locations.len();
306        let total_pairs = n * (n - 1);
307        let mut geometries = HashMap::new();
308        let mut pair_count = 0usize;
309
310        for i in 0..n {
311            for j in 0..n {
312                if i == j {
313                    continue;
314                }
315                if let Ok(result) = self.route(locations[i], locations[j]) {
316                    geometries.insert((i, j), result.geometry);
317                }
318                pair_count += 1;
319
320                if let Some(tx) = progress {
321                    if pair_count.is_multiple_of(10) || pair_count == total_pairs {
322                        let percent = 80 + (pair_count * 15 / total_pairs.max(1)) as u8;
323                        let _ = tx
324                            .send(RoutingProgress::ComputingGeometries {
325                                percent,
326                                pair: pair_count,
327                                total: total_pairs,
328                            })
329                            .await;
330                    }
331                }
332            }
333        }
334
335        geometries
336    }
337}
solverforge_maps/routing/matrix.rs

solverforge_maps/routing/
matrix.rs
