axiolid_measure/
frechet.rs

1//! Fréchet distance between polylines (#147).
2//!
3//! The Fréchet distance is the least leash length that lets two walkers
4//! traverse the two curves from start to end, each moving only forward. The
5//! discrete version walks vertex to vertex; the continuous version walks
6//! along the segments too, and is never larger.
7//!
8//! - [`discrete_frechet_distance`]: the Eiter-Mannila dynamic programme,
9//!   `O(nm)` time and `O(m)` memory, iterative.
10//! - [`frechet_at_most`]: the Alt-Godau decision, `O(nm)`: can the curves be
11//!   walked with a leash of length `eps`?
12//! - [`frechet_distance`]: the continuous distance. The answer is always one
13//!   of the critical values of the free space -- a distance between two
14//!   vertices, from a vertex to a segment, or from a point on a segment to
15//!   two vertices of the other curve at once -- so it is the least critical
16//!   value the decision accepts, found by binary search over the sorted
17//!   critical values. When there are more than [`CANDIDATE_BUDGET`] of them
18//!   (there are `O(n^2 m + n m^2)`), the range is first narrowed by bisection
19//!   on the decision, so memory stays bounded.
20//!
21//! The `_2d` functions lift the points to `z = 0`, which changes no distance.
22//!
23//! # Rounding
24//!
25//! Everything is `f64`. Critical values are computed in floating point, and
26//! the decision reads its free-space intervals with the same helpers, so at
27//! a vertex-to-vertex or vertex-to-segment critical value the decision sees
28//! exactly the distance the candidate was built from. A passage that opens
29//! at a point equidistant to two vertices is decided by comparing interval
30//! ends, which carry a few units of rounding; there the answer can be the
31//! next larger critical value. Nothing here is certified.
32
33use core::fmt;
34
35use axiolid_core::{Point2, Point3};
36
37/// Critical values held at once before the range is narrowed by bisection.
38pub const CANDIDATE_BUDGET: usize = 1 << 20;
39
40/// Why a Fréchet query was refused.
41#[derive(Debug, Clone, Copy, PartialEq, Eq)]
42pub enum FrechetError {
43    /// A polyline has no points.
44    EmptyPolyline,
45    /// A coordinate is NaN or infinite.
46    NonFiniteInput,
47    /// The leash length for [`frechet_at_most`] is negative or not finite.
48    InvalidBound,
49}
50
51impl fmt::Display for FrechetError {
52    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
53        match self {
54            Self::EmptyPolyline => formatter.write_str("a polyline has no points"),
55            Self::NonFiniteInput => formatter.write_str("polyline coordinates must be finite"),
56            Self::InvalidBound => {
57                formatter.write_str("the leash length must be finite and non-negative")
58            }
59        }
60    }
61}
62
63impl std::error::Error for FrechetError {}
64
65/// Discrete Fréchet distance between the vertex sequences of `a` and `b`.
66///
67/// # Errors
68///
69/// An empty polyline or a non-finite coordinate.
70pub fn discrete_frechet_distance(a: &[Point3], b: &[Point3]) -> Result<f64, FrechetError> {
71    check(a)?;
72    check(b)?;
73    Ok(discrete(a, b))
74}
75
76/// [`discrete_frechet_distance`] in the plane.
77///
78/// # Errors
79///
80/// As [`discrete_frechet_distance`].
81pub fn discrete_frechet_distance_2d(a: &[Point2], b: &[Point2]) -> Result<f64, FrechetError> {
82    discrete_frechet_distance(&lift(a), &lift(b))
83}
84
85/// Whether the continuous Fréchet distance between `a` and `b` is at most
86/// `eps`.
87///
88/// # Errors
89///
90/// An empty polyline, a non-finite coordinate, or a negative or non-finite
91/// `eps`.
92pub fn frechet_at_most(a: &[Point3], b: &[Point3], eps: f64) -> Result<bool, FrechetError> {
93    check(a)?;
94    check(b)?;
95    if !eps.is_finite() || eps < 0.0 {
96        return Err(FrechetError::InvalidBound);
97    }
98    Ok(decide(a, b, eps))
99}
100
101/// [`frechet_at_most`] in the plane.
102///
103/// # Errors
104///
105/// As [`frechet_at_most`].
106pub fn frechet_at_most_2d(a: &[Point2], b: &[Point2], eps: f64) -> Result<bool, FrechetError> {
107    frechet_at_most(&lift(a), &lift(b), eps)
108}
109
110/// Continuous Fréchet distance between the polylines `a` and `b`.
111///
112/// # Errors
113///
114/// An empty polyline or a non-finite coordinate.
115pub fn frechet_distance(a: &[Point3], b: &[Point3]) -> Result<f64, FrechetError> {
116    check(a)?;
117    check(b)?;
118    Ok(continuous(a, b, CANDIDATE_BUDGET))
119}
120
121/// [`frechet_distance`] in the plane.
122///
123/// # Errors
124///
125/// As [`frechet_distance`].
126pub fn frechet_distance_2d(a: &[Point2], b: &[Point2]) -> Result<f64, FrechetError> {
127    frechet_distance(&lift(a), &lift(b))
128}
129
130fn check(points: &[Point3]) -> Result<(), FrechetError> {
131    if points.is_empty() {
132        return Err(FrechetError::EmptyPolyline);
133    }
134    if points.iter().any(|p| !p.is_finite()) {
135        return Err(FrechetError::NonFiniteInput);
136    }
137    Ok(())
138}
139
140fn lift(points: &[Point2]) -> Vec<Point3> {
141    points.iter().map(|p| Point3::new(p.x, p.y, 0.0)).collect()
142}
143
144fn distance(p: Point3, q: Point3) -> f64 {
145    (p - q).length()
146}
147
148fn discrete(a: &[Point3], b: &[Point3]) -> f64 {
149    // `row[j]` is the coupling cost of a[..=i] with b[..=j].
150    let mut row = vec![0.0_f64; b.len()];
151    for (i, &p) in a.iter().enumerate() {
152        let mut diagonal = 0.0;
153        for (j, &q) in b.iter().enumerate() {
154            let here = distance(p, q);
155            let above = row[j];
156            let best = match (i, j) {
157                (0, 0) => 0.0,
158                (0, _) => row[j - 1],
159                (_, 0) => above,
160                _ => above.min(diagonal).min(row[j - 1]),
161            };
162            diagonal = above;
163            row[j] = here.max(best);
164        }
165    }
166    row[b.len() - 1]
167}
168
169/// The foot of `c` on the line through segment `(a, b)`: its parameter
170/// (unclamped) and its distance to `c`. `None` for a zero-length segment.
171fn foot(c: Point3, a: Point3, b: Point3) -> Option<(f64, f64)> {
172    let d = b - a;
173    let length_squared = d.length_squared();
174    if length_squared == 0.0 {
175        return None;
176    }
177    let t = (c - a).dot(d) / length_squared;
178    Some((t, distance(a + d * t, c)))
179}
180
181/// A closed parameter interval on a segment, empty when `lo > hi`.
182#[derive(Clone, Copy)]
183struct Span {
184    lo: f64,
185    hi: f64,
186}
187
188impl Span {
189    const EMPTY: Span = Span {
190        lo: f64::INFINITY,
191        hi: f64::NEG_INFINITY,
192    };
193
194    fn is_empty(self) -> bool {
195        self.lo > self.hi
196    }
197}
198
199/// The parameters of segment `(a, b)` within `eps` of `c`.
200///
201/// Endpoints are decided by the same vertex distance the vertex-to-vertex
202/// critical values are built from, and the interior by the same foot
203/// distance the vertex-to-segment ones are, so at a critical value the
204/// interval opens exactly.
205fn free(c: Point3, a: Point3, b: Point3, eps: f64) -> Span {
206    let start = distance(a, c) <= eps;
207    let end = distance(b, c) <= eps;
208    let Some((t, h)) = foot(c, a, b) else {
209        return if start {
210            Span { lo: 0.0, hi: 1.0 }
211        } else {
212            Span::EMPTY
213        };
214    };
215    if h > eps {
216        return Span::EMPTY;
217    }
218    let w = (eps * eps - h * h).max(0.0).sqrt() / (b - a).length();
219    let lo = if start { 0.0 } else { (t - w).max(0.0) };
220    let hi = if end { 1.0 } else { (t + w).min(1.0) };
221    if lo > hi {
222        Span::EMPTY
223    } else {
224        Span { lo, hi }
225    }
226}
227
228/// Alt-Godau: is the top-right corner of the free space reachable from the
229/// bottom-left by a monotone path?
230fn decide(a: &[Point3], b: &[Point3], eps: f64) -> bool {
231    if distance(a[0], b[0]) > eps || distance(a[a.len() - 1], b[b.len() - 1]) > eps {
232        return false;
233    }
234    let (p, q) = (a.len() - 1, b.len() - 1);
235    if p == 0 || q == 0 {
236        // One curve is a point: every point of the other must be within
237        // `eps` of it, and a segment's farthest point is an endpoint.
238        let (point, other) = if p == 0 { (a[0], b) } else { (b[0], a) };
239        return other.iter().all(|&v| distance(point, v) <= eps);
240    }
241    // `left[j]`: reachable part of the left edge of cell (i, j), a span of
242    // b's segment j at a's vertex i. `bottom`: reachable part of the bottom
243    // edge of cell (i, j), a span of a's segment i at b's vertex j.
244    let mut left: Vec<Span> = Vec::with_capacity(q);
245    let mut open = true;
246    for j in 0..q {
247        let span = free(a[0], b[j], b[j + 1], eps);
248        left.push(if open && span.lo == 0.0 {
249            span
250        } else {
251            Span::EMPTY
252        });
253        open = open && !span.is_empty() && span.lo == 0.0 && span.hi == 1.0;
254    }
255    let mut bottom_open = true;
256    for i in 0..p {
257        // Bottom edge of cell (i, 0): along the start of b, as for `left`.
258        let span = free(b[0], a[i], a[i + 1], eps);
259        let mut bottom = if bottom_open && span.lo == 0.0 {
260            span
261        } else {
262            Span::EMPTY
263        };
264        bottom_open = bottom_open && !span.is_empty() && span.lo == 0.0 && span.hi == 1.0;
265        for j in 0..q {
266            let l = left[j];
267            let top = free(b[j + 1], a[i], a[i + 1], eps);
268            let right = free(a[i + 1], b[j], b[j + 1], eps);
269            let next_bottom = if !l.is_empty() {
270                top
271            } else if !bottom.is_empty() {
272                Span {
273                    lo: bottom.lo.max(top.lo),
274                    hi: top.hi,
275                }
276            } else {
277                Span::EMPTY
278            };
279            left[j] = if !bottom.is_empty() {
280                right
281            } else if !l.is_empty() {
282                Span {
283                    lo: l.lo.max(right.lo),
284                    hi: right.hi,
285                }
286            } else {
287                Span::EMPTY
288            };
289            bottom = next_bottom;
290        }
291        // `bottom` is now the top edge of cell (i, q - 1).
292        if i == p - 1 {
293            return (!bottom.is_empty() && bottom.hi == 1.0)
294                || (!left[q - 1].is_empty() && left[q - 1].hi == 1.0);
295        }
296    }
297    unreachable!("p >= 1 returns in the loop")
298}
299
300/// Every critical value: vertex-vertex, vertex-segment, and the points of a
301/// segment equidistant to two vertices of the other curve.
302fn for_each_candidate(a: &[Point3], b: &[Point3], mut visit: impl FnMut(f64)) {
303    for &p in a {
304        for &q in b {
305            visit(distance(p, q));
306        }
307    }
308    for (points, other) in [(a, b), (b, a)] {
309        for &c in points {
310            for s in other.windows(2) {
311                if let Some((t, h)) = foot(c, s[0], s[1]) {
312                    if (0.0..=1.0).contains(&t) {
313                        visit(h);
314                    }
315                }
316            }
317        }
318        for s in other.windows(2) {
319            let (start, d) = (s[0], s[1] - s[0]);
320            for k in 0..points.len() {
321                for l in k + 1..points.len() {
322                    let n = points[l] - points[k];
323                    let along = n.dot(d);
324                    if along == 0.0 {
325                        continue;
326                    }
327                    let middle = (points[k] + points[l]) * 0.5;
328                    let t = n.dot(middle - start) / along;
329                    if (0.0..=1.0).contains(&t) {
330                        let x = start + d * t;
331                        visit(distance(x, points[k]).max(distance(x, points[l])));
332                    }
333                }
334            }
335        }
336    }
337}
338
339fn continuous(a: &[Point3], b: &[Point3], budget: usize) -> f64 {
340    // The endpoints must be coupled, so their distances bound from below.
341    let mut lo = distance(a[0], b[0]).max(distance(a[a.len() - 1], b[b.len() - 1]));
342    if decide(a, b, lo) {
343        return lo;
344    }
345    // The discrete coupling, interpolated, is a continuous one.
346    let mut hi = discrete(a, b);
347    // Invariant: the answer lies in (lo, hi] and `decide(hi)` holds.
348    loop {
349        let mut count = 0usize;
350        for_each_candidate(a, b, |c| {
351            if c > lo && c <= hi {
352                count += 1;
353            }
354        });
355        let mid = lo + (hi - lo) * 0.5;
356        if count <= budget || mid <= lo || mid >= hi {
357            break;
358        }
359        if decide(a, b, mid) {
360            hi = mid;
361        } else {
362            lo = mid;
363        }
364    }
365    let mut candidates = Vec::new();
366    for_each_candidate(a, b, |c| {
367        if c > lo && c <= hi {
368            candidates.push(c);
369        }
370    });
371    candidates.sort_by(f64::total_cmp);
372    candidates.dedup();
373    // Least accepted candidate; the decision is monotone in `eps`.
374    let first = candidates.partition_point(|&c| !decide(a, b, c));
375    candidates.get(first).copied().unwrap_or(hi)
376}
377
378#[cfg(test)]
379mod tests {
380    use super::*;
381
382    /// A deterministic pseudo-random walk of `n` points in space.
383    fn walk(seed: u64, n: usize) -> Vec<Point3> {
384        let mut state = seed;
385        let mut next = || {
386            state = state
387                .wrapping_mul(6364136223846793005)
388                .wrapping_add(1442695040888963407);
389            ((state >> 11) as f64 / (1u64 << 53) as f64) * 2.0 - 1.0
390        };
391        let mut p = Point3::ZERO;
392        (0..n)
393            .map(|_| {
394                p += Point3::new(1.0 + next(), next(), next());
395                p
396            })
397            .collect()
398    }
399
400    /// Narrowing the range by bisection before listing candidates finds
401    /// the same critical value as listing them all at once.
402    #[test]
403    fn a_tiny_budget_bisects_to_the_same_answer() {
404        for seed in 0..16 {
405            let a = walk(seed, 12);
406            let jitter = walk(seed + 50, 12);
407            let b: Vec<Point3> = (0..12)
408                .map(|i| {
409                    if i == 0 || i == 11 {
410                        a[i]
411                    } else {
412                        a[i] + (jitter[i] - jitter[i - 1]) * 0.4
413                    }
414                })
415                .collect();
416            let listed = continuous(&a, &b, usize::MAX);
417            let mut bisections = 0;
418            let mut lo = 0.0_f64;
419            let mut hi = discrete(&a, &b);
420            // The same narrowing as `continuous`, counted, to show it runs.
421            loop {
422                let mut count = 0usize;
423                for_each_candidate(&a, &b, |c| {
424                    if c > lo && c <= hi {
425                        count += 1;
426                    }
427                });
428                let mid = lo + (hi - lo) * 0.5;
429                if count <= 2 || mid <= lo || mid >= hi {
430                    break;
431                }
432                bisections += 1;
433                if decide(&a, &b, mid) {
434                    hi = mid;
435                } else {
436                    lo = mid;
437                }
438            }
439            assert!(bisections > 0, "{seed}");
440            assert_eq!(continuous(&a, &b, 2), listed, "{seed}");
441            assert!(listed > 0.0);
442        }
443    }
444}
axiolid_measure/frechet.rs

axiolid_measure/
frechet.rs
