Skip to main content

fieldopt_core/
distance.rs

1//! Travel matrices from coordinates.
2//!
3//! A [`Matrix`] is a dense, row-major `n x n` matrix of travel distances or
4//! times. Users with a road-network matrix (for example from OSRM) build one
5//! with [`Matrix::from_vec`]; otherwise [`haversine_matrix`] (great-circle
6//! distance in kilometres from latitude and longitude in degrees) or
7//! [`euclidean_matrix`] (planar coordinates in any unit) give a first
8//! approximation.
9
10/// Dense square matrix of travel costs between units.
11#[derive(Clone, Debug, PartialEq)]
12pub struct Matrix {
13    n: usize,
14    data: Vec<f64>,
15}
16
17impl Matrix {
18    /// Build from a row-major vector of length `n * n`.
19    pub fn from_vec(n: usize, data: Vec<f64>) -> Result<Self, String> {
20        if data.len() != n * n {
21            return Err(format!(
22                "expected {} values for an {n} x {n} matrix, got {}",
23                n * n,
24                data.len()
25            ));
26        }
27        if data.iter().any(|v| !v.is_finite() || *v < 0.0) {
28            return Err("travel costs must be finite and non-negative".into());
29        }
30        Ok(Matrix { n, data })
31    }
32
33    /// Number of units.
34    pub fn len(&self) -> usize {
35        self.n
36    }
37
38    /// Whether the matrix is empty.
39    pub fn is_empty(&self) -> bool {
40        self.n == 0
41    }
42
43    /// Travel cost from `i` to `j`.
44    #[inline]
45    pub fn get(&self, i: usize, j: usize) -> f64 {
46        self.data[i * self.n + j]
47    }
48
49    /// The underlying row-major data.
50    pub fn as_slice(&self) -> &[f64] {
51        &self.data
52    }
53}
54
55/// Earth radius used by [`haversine_matrix`], in kilometres.
56pub const EARTH_RADIUS_KM: f64 = 6371.0088;
57
58/// Great-circle distances in kilometres between points given by latitude and
59/// longitude in degrees.
60pub fn haversine_matrix(lat: &[f64], lon: &[f64]) -> Result<Matrix, String> {
61    if lat.len() != lon.len() {
62        return Err("latitude and longitude must have the same length".into());
63    }
64    if lat.iter().any(|v| !(-90.0..=90.0).contains(v))
65        || lon.iter().any(|v| !(-180.0..=180.0).contains(v))
66    {
67        return Err("latitude must be in [-90, 90] and longitude in [-180, 180] degrees".into());
68    }
69    let n = lat.len();
70    let (la, lo): (Vec<f64>, Vec<f64>) = (
71        lat.iter().map(|v| v.to_radians()).collect(),
72        lon.iter().map(|v| v.to_radians()).collect(),
73    );
74    let mut data = vec![0.0; n * n];
75    for i in 0..n {
76        for j in (i + 1)..n {
77            let dlat = la[j] - la[i];
78            let dlon = lo[j] - lo[i];
79            let a =
80                (dlat / 2.0).sin().powi(2) + la[i].cos() * la[j].cos() * (dlon / 2.0).sin().powi(2);
81            let d = 2.0 * EARTH_RADIUS_KM * a.sqrt().asin();
82            data[i * n + j] = d;
83            data[j * n + i] = d;
84        }
85    }
86    Ok(Matrix { n, data })
87}
88
89/// Euclidean distances between planar points.
90pub fn euclidean_matrix(x: &[f64], y: &[f64]) -> Result<Matrix, String> {
91    if x.len() != y.len() {
92        return Err("x and y must have the same length".into());
93    }
94    if x.iter().chain(y.iter()).any(|v| !v.is_finite()) {
95        return Err("coordinates must be finite".into());
96    }
97    let n = x.len();
98    let mut data = vec![0.0; n * n];
99    for i in 0..n {
100        for j in (i + 1)..n {
101            let d = ((x[i] - x[j]).powi(2) + (y[i] - y[j]).powi(2)).sqrt();
102            data[i * n + j] = d;
103            data[j * n + i] = d;
104        }
105    }
106    Ok(Matrix { n, data })
107}
108
109#[cfg(test)]
110mod tests {
111    use super::*;
112
113    #[test]
114    fn haversine_known_distance() {
115        // Recife (-8.05, -34.9) to Salvador (-12.97, -38.51): about 675 km
116        let m = haversine_matrix(&[-8.05, -12.97], &[-34.9, -38.51]).unwrap();
117        assert!((m.get(0, 1) - 675.0).abs() < 5.0);
118        assert_eq!(m.get(0, 1), m.get(1, 0));
119        assert_eq!(m.get(0, 0), 0.0);
120    }
121
122    #[test]
123    fn euclidean_and_validation() {
124        let m = euclidean_matrix(&[0.0, 3.0], &[0.0, 4.0]).unwrap();
125        assert_eq!(m.get(0, 1), 5.0);
126        assert!(Matrix::from_vec(2, vec![0.0, 1.0, 1.0]).is_err());
127        assert!(Matrix::from_vec(2, vec![0.0, -1.0, 1.0, 0.0]).is_err());
128        assert!(haversine_matrix(&[95.0], &[0.0]).is_err());
129    }
130}