osmnxr_core/
spatial.rs

1//! Planar point-to-polyline snapping over a uniform grid index.
2//!
3//! Edges arrive as flattened vertex arrays plus per-edge offsets (the R layer
4//! passes `sf::st_coordinates()` output). Coordinates are treated as planar;
5//! the R layer owns CRS semantics.
6
7use std::collections::HashMap;
8
9/// One polyline segment with its owning edge and the cumulative planar
10/// length of the polyline before this segment.
11struct Seg {
12    x1: f64,
13    y1: f64,
14    x2: f64,
15    y2: f64,
16    edge: u32,
17    pos0: f64,
18}
19
20/// The nearest location on the edge set to a query point.
21pub struct Snap {
22    /// 0-based edge index.
23    pub edge: usize,
24    /// Planar distance from the query point to the snapped location.
25    pub dist: f64,
26    /// Snapped coordinates (on the edge polyline).
27    pub x: f64,
28    pub y: f64,
29    /// Planar distance along the edge polyline from its first vertex.
30    pub pos: f64,
31}
32
33/// A uniform grid over the segments of a set of polylines, sized for about
34/// one segment per cell. Street-network segments are short relative to the
35/// extent, so each occupies a handful of cells; the few that span many cells
36/// go into an `oversize` list scanned on every query.
37pub struct SegmentIndex {
38    segs: Vec<Seg>,
39    grid: HashMap<(i64, i64), Vec<u32>>,
40    oversize: Vec<u32>,
41    cell: f64,
42    minx: f64,
43    miny: f64,
44    maxx: f64,
45    maxy: f64,
46}
47
48/// Squared distance from `(px, py)` to the segment, the clamped projection
49/// parameter `t`, the projected point, and the segment length.
50fn project(px: f64, py: f64, s: &Seg) -> (f64, f64, f64, f64, f64) {
51    let dx = s.x2 - s.x1;
52    let dy = s.y2 - s.y1;
53    let len2 = dx * dx + dy * dy;
54    let t = if len2 > 0.0 {
55        (((px - s.x1) * dx + (py - s.y1) * dy) / len2).clamp(0.0, 1.0)
56    } else {
57        0.0
58    };
59    let x = s.x1 + t * dx;
60    let y = s.y1 + t * dy;
61    let d2 = (px - x) * (px - x) + (py - y) * (py - y);
62    (d2, t, x, y, len2.sqrt())
63}
64
65impl SegmentIndex {
66    pub fn build(vx: &[f64], vy: &[f64], offsets: &[usize]) -> Self {
67        let n_edges = offsets.len().saturating_sub(1);
68        let mut segs: Vec<Seg> = Vec::new();
69        let (mut minx, mut miny) = (f64::INFINITY, f64::INFINITY);
70        let (mut maxx, mut maxy) = (f64::NEG_INFINITY, f64::NEG_INFINITY);
71        for e in 0..n_edges {
72            let mut pos0 = 0.0;
73            let hi = offsets[e + 1];
74            for i in offsets[e]..hi.saturating_sub(1) {
75                let (x1, y1, x2, y2) = (vx[i], vy[i], vx[i + 1], vy[i + 1]);
76                minx = minx.min(x1.min(x2));
77                miny = miny.min(y1.min(y2));
78                maxx = maxx.max(x1.max(x2));
79                maxy = maxy.max(y1.max(y2));
80                let len = ((x2 - x1).powi(2) + (y2 - y1).powi(2)).sqrt();
81                segs.push(Seg {
82                    x1,
83                    y1,
84                    x2,
85                    y2,
86                    edge: e as u32,
87                    pos0,
88                });
89                pos0 += len;
90            }
91        }
92
93        let w = (maxx - minx).max(0.0);
94        let h = (maxy - miny).max(0.0);
95        let m = segs.len().max(1) as f64;
96        let mut cell = (w * h / m).sqrt();
97        if !cell.is_finite() || cell <= 0.0 {
98            cell = w.max(h) / m.sqrt(); // degenerate (collinear) extents
99        }
100        if !cell.is_finite() || cell <= 0.0 {
101            cell = 1.0; // single location or empty
102        }
103
104        let mut grid: HashMap<(i64, i64), Vec<u32>> = HashMap::new();
105        let mut oversize: Vec<u32> = Vec::new();
106        for (i, s) in segs.iter().enumerate() {
107            let ix0 = (s.x1.min(s.x2) / cell).floor() as i64;
108            let ix1 = (s.x1.max(s.x2) / cell).floor() as i64;
109            let iy0 = (s.y1.min(s.y2) / cell).floor() as i64;
110            let iy1 = (s.y1.max(s.y2) / cell).floor() as i64;
111            let n_cells = (ix1 - ix0 + 1).saturating_mul(iy1 - iy0 + 1);
112            if n_cells > 1024 {
113                oversize.push(i as u32);
114                continue;
115            }
116            for ix in ix0..=ix1 {
117                for iy in iy0..=iy1 {
118                    grid.entry((ix, iy)).or_default().push(i as u32);
119                }
120            }
121        }
122        SegmentIndex {
123            segs,
124            grid,
125            oversize,
126            cell,
127            minx,
128            miny,
129            maxx,
130            maxy,
131        }
132    }
133
134    #[cfg(test)]
135    pub fn is_empty(&self) -> bool {
136        self.segs.is_empty()
137    }
138
139    /// Nearest edge location to `(px, py)` within `tolerance` (pass
140    /// `f64::INFINITY` for unlimited). Ties resolve to the lowest segment
141    /// index, so results are deterministic.
142    pub fn nearest(&self, px: f64, py: f64, tolerance: f64) -> Option<Snap> {
143        if self.segs.is_empty() {
144            return None;
145        }
146        // best = (d2, seg index, t, x, y)
147        let mut best: Option<(f64, u32, f64, f64, f64)> = None;
148        let consider = |si: u32, best: &mut Option<(f64, u32, f64, f64, f64)>| {
149            let s = &self.segs[si as usize];
150            let (d2, t, x, y, _) = project(px, py, s);
151            let better = match best {
152                None => true,
153                Some((bd2, bsi, ..)) => d2 < *bd2 || (d2 == *bd2 && si < *bsi),
154            };
155            if better {
156                *best = Some((d2, si, t, x, y));
157            }
158        };
159        for &si in &self.oversize {
160            consider(si, &mut best);
161        }
162
163        let cell = self.cell;
164        // Ring-search around the query cell, clamped per axis into the range
165        // of occupied cells: clamping can only shrink the per-axis cell
166        // distance to any occupied cell, so the (r - 1) * cell ring lower
167        // bound below stays valid, while far-away points (think wrong CRS)
168        // cost a bounded number of rings instead of one per cell of distance.
169        let lo_x = (self.minx / cell).floor() as i64;
170        let hi_x = (self.maxx / cell).floor() as i64;
171        let lo_y = (self.miny / cell).floor() as i64;
172        let hi_y = (self.maxy / cell).floor() as i64;
173        let cx = ((px / cell).floor() as i64).clamp(lo_x, hi_x);
174        let cy = ((py / cell).floor() as i64).clamp(lo_y, hi_y);
175        // Rings beyond this cover every cell the data occupies.
176        let max_ring = (hi_x - lo_x).max(hi_y - lo_y) + 1;
177
178        for r in 0..=max_ring {
179            // No cell at Chebyshev ring r can hold anything closer than this.
180            let bound = if r <= 1 { 0.0 } else { (r - 1) as f64 * cell };
181            if let Some((bd2, ..)) = best {
182                if bd2 <= bound * bound {
183                    break;
184                }
185            }
186            if bound > tolerance {
187                break;
188            }
189            let scan = |ix: i64, iy: i64, best: &mut Option<(f64, u32, f64, f64, f64)>| {
190                if let Some(cellv) = self.grid.get(&(ix, iy)) {
191                    for &si in cellv {
192                        consider(si, best);
193                    }
194                }
195            };
196            if r == 0 {
197                scan(cx, cy, &mut best);
198            } else {
199                for ix in (cx - r)..=(cx + r) {
200                    scan(ix, cy - r, &mut best);
201                    scan(ix, cy + r, &mut best);
202                }
203                for iy in (cy - r + 1)..=(cy + r - 1) {
204                    scan(cx - r, iy, &mut best);
205                    scan(cx + r, iy, &mut best);
206                }
207            }
208        }
209
210        best.and_then(|(d2, si, t, x, y)| {
211            let dist = d2.sqrt();
212            if dist > tolerance {
213                return None;
214            }
215            let s = &self.segs[si as usize];
216            let seg_len = ((s.x2 - s.x1).powi(2) + (s.y2 - s.y1).powi(2)).sqrt();
217            Some(Snap {
218                edge: s.edge as usize,
219                dist,
220                x,
221                y,
222                pos: s.pos0 + t * seg_len,
223            })
224        })
225    }
226}
227
228#[cfg(test)]
229mod tests {
230    use super::*;
231
232    /// Brute-force nearest over all segments, mirroring `nearest`'s
233    /// tie-breaking (lowest segment index).
234    fn brute(vx: &[f64], vy: &[f64], offsets: &[usize], px: f64, py: f64) -> (usize, f64, f64) {
235        let mut best: Option<(f64, usize, f64)> = None; // d2, edge, pos
236        for e in 0..offsets.len() - 1 {
237            let mut pos0 = 0.0;
238            for i in offsets[e]..offsets[e + 1] - 1 {
239                let s = Seg {
240                    x1: vx[i],
241                    y1: vy[i],
242                    x2: vx[i + 1],
243                    y2: vy[i + 1],
244                    edge: e as u32,
245                    pos0,
246                };
247                let (d2, t, _, _, seg_len) = project(px, py, &s);
248                if best.map_or(true, |(bd2, ..)| d2 < bd2) {
249                    best = Some((d2, e, pos0 + t * seg_len));
250                }
251                pos0 += seg_len;
252            }
253        }
254        let (d2, e, pos) = best.unwrap();
255        (e, d2.sqrt(), pos)
256    }
257
258    /// Tiny deterministic xorshift64 RNG (same as routing's).
259    struct Rng(u64);
260    impl Rng {
261        fn next(&mut self) -> u64 {
262            let mut x = self.0;
263            x ^= x << 13;
264            x ^= x >> 7;
265            x ^= x << 17;
266            self.0 = x;
267            x
268        }
269        fn unif(&mut self) -> f64 {
270            (self.next() % 1_000_000) as f64 / 1_000_000.0
271        }
272    }
273
274    #[test]
275    fn snaps_to_the_interior_of_a_segment() {
276        // One horizontal edge from (0,0) to (10,0).
277        let vx = vec![0.0, 10.0];
278        let vy = vec![0.0, 0.0];
279        let idx = SegmentIndex::build(&vx, &vy, &[0, 2]);
280        let s = idx.nearest(3.0, 4.0, f64::INFINITY).unwrap();
281        assert_eq!(s.edge, 0);
282        assert!((s.dist - 4.0).abs() < 1e-12);
283        assert!((s.x - 3.0).abs() < 1e-12 && s.y.abs() < 1e-12);
284        assert!((s.pos - 3.0).abs() < 1e-12);
285    }
286
287    #[test]
288    fn clamps_beyond_the_ends() {
289        let vx = vec![0.0, 10.0];
290        let vy = vec![0.0, 0.0];
291        let idx = SegmentIndex::build(&vx, &vy, &[0, 2]);
292        let s = idx.nearest(-5.0, 1.0, f64::INFINITY).unwrap();
293        assert!((s.pos - 0.0).abs() < 1e-12);
294        let s = idx.nearest(15.0, -1.0, f64::INFINITY).unwrap();
295        assert!((s.pos - 10.0).abs() < 1e-12);
296    }
297
298    #[test]
299    fn respects_the_tolerance() {
300        let vx = vec![0.0, 10.0];
301        let vy = vec![0.0, 0.0];
302        let idx = SegmentIndex::build(&vx, &vy, &[0, 2]);
303        assert!(idx.nearest(5.0, 3.0, 2.0).is_none());
304        assert!(idx.nearest(5.0, 3.0, 3.0).is_some());
305    }
306
307    #[test]
308    fn position_accumulates_over_polyline_vertices() {
309        // L-shaped edge: (0,0) -> (10,0) -> (10,10).
310        let vx = vec![0.0, 10.0, 10.0];
311        let vy = vec![0.0, 0.0, 10.0];
312        let idx = SegmentIndex::build(&vx, &vy, &[0, 3]);
313        let s = idx.nearest(12.0, 4.0, f64::INFINITY).unwrap();
314        assert_eq!(s.edge, 0);
315        assert!((s.pos - 14.0).abs() < 1e-12);
316        assert!((s.x - 10.0).abs() < 1e-12 && (s.y - 4.0).abs() < 1e-12);
317    }
318
319    #[test]
320    fn empty_index_returns_none() {
321        let idx = SegmentIndex::build(&[], &[], &[0]);
322        assert!(idx.is_empty());
323        assert!(idx.nearest(0.0, 0.0, f64::INFINITY).is_none());
324    }
325
326    #[test]
327    fn matches_brute_force_on_random_networks() {
328        let mut rng = Rng(42);
329        for _ in 0..5 {
330            // ~80 random 2- to 4-vertex polylines in the unit square.
331            let n_edges = 80;
332            let mut vx = Vec::new();
333            let mut vy = Vec::new();
334            let mut offsets = vec![0usize];
335            for _ in 0..n_edges {
336                let nv = 2 + (rng.next() % 3) as usize;
337                for _ in 0..nv {
338                    vx.push(rng.unif());
339                    vy.push(rng.unif());
340                }
341                offsets.push(vx.len());
342            }
343            let idx = SegmentIndex::build(&vx, &vy, &offsets);
344            for _ in 0..200 {
345                // include points outside the extent
346                let px = rng.unif() * 1.6 - 0.3;
347                let py = rng.unif() * 1.6 - 0.3;
348                let got = idx.nearest(px, py, f64::INFINITY).unwrap();
349                let (we, wd, wp) = brute(&vx, &vy, &offsets, px, py);
350                assert!(
351                    (got.dist - wd).abs() < 1e-12,
352                    "dist mismatch at ({px}, {py}): got {} want {wd}",
353                    got.dist
354                );
355                // Equidistant snaps may differ in edge; the distance must not.
356                if got.edge == we {
357                    assert!((got.pos - wp).abs() < 1e-9);
358                }
359            }
360        }
361    }
362}
osmnxr_core/spatial.rs

osmnxr_core/
spatial.rs
