fast_mesh_query/
closest.rs

1//! Closest-point queries against a mesh, accelerated by its BVH.
2
3use std::cell::Cell;
4
5use fast_mesh_bvh::{Aabb, Bvh, NodeKind};
6use fast_mesh_store::{Mesh, TriangleId, par};
7
8use crate::ray::{PAR_MIN_QUERIES, QUERY_CHUNK, points};
9use crate::tri::{TriClosest, closest_on_triangle};
10use crate::{Feature, QueryError, Result, check};
11
12/// Options for [`closest_point`].
13#[derive(Debug, Clone, Copy, PartialEq)]
14pub struct ClosestOptions {
15    /// Largest distance that counts as found (inclusive). Default
16    /// `f32::INFINITY`.
17    pub max_distance: f32,
18}
19
20impl Default for ClosestOptions {
21    fn default() -> Self {
22        ClosestOptions {
23            max_distance: f32::INFINITY,
24        }
25    }
26}
27
28/// The point of a mesh nearest to a query point.
29#[derive(Debug, Clone, Copy, PartialEq)]
30pub struct ClosestHit {
31    /// The triangle the point lies on.
32    pub triangle: TriangleId,
33    /// The nearest point, in mesh-local coordinates.
34    pub point: [f32; 3],
35    /// Distance from the query to `point`.
36    pub distance: f32,
37    /// Weights of the triangle's corners 0, 1 and 2 at `point`.
38    pub barycentric: [f32; 3],
39    /// The vertex, edge or face of the triangle that `point` lies on.
40    pub feature: Feature,
41}
42
43/// Squared distance from `q` to the box `b`; zero when `q` is inside.
44pub(crate) fn box_dist2(q: [f32; 3], b: &Aabb) -> f32 {
45    let mut d2 = 0.0;
46    for (i, &c) in q.iter().enumerate() {
47        let d = (b.min[i] - c).max(c - b.max[i]).max(0.0);
48        d2 += d * d;
49    }
50    d2
51}
52
53fn validate_point(p: [f32; 3]) -> Result<()> {
54    if !p.iter().all(|c| c.is_finite()) {
55        return Err(QueryError::InvalidInput("query point must be finite"));
56    }
57    Ok(())
58}
59
60fn validate(p: [f32; 3], opts: &ClosestOptions) -> Result<()> {
61    validate_point(p)?;
62    validate_opts(opts)
63}
64
65fn validate_opts(opts: &ClosestOptions) -> Result<()> {
66    if opts.max_distance.is_nan() || opts.max_distance < 0.0 {
67        return Err(QueryError::InvalidInput(
68            "max_distance must be zero or greater, and not NaN",
69        ));
70    }
71    Ok(())
72}
73
74fn closest(mesh: &Mesh, bvh: &Bvh, q: [f32; 3], opts: &ClosestOptions) -> Option<ClosestHit> {
75    let max2 = opts.max_distance * opts.max_distance;
76    let best: Cell<Option<(TriangleId, TriClosest<f32>)>> = Cell::new(None);
77    let best_dist2 = || best.get().map_or(max2, |(_, c)| c.dist2);
78    bvh.traverse_by_key(
79        |b| {
80            let d2 = box_dist2(q, b);
81            (d2 <= max2).then_some(d2)
82        },
83        |_, node, d2| {
84            // Boxes at exactly the best distance are still visited, so that a
85            // tie can go to the smaller id.
86            if d2 > best_dist2() {
87                return false;
88            }
89            if let NodeKind::Leaf { triangles } = node.kind {
90                for &id in triangles {
91                    let c = closest_on_triangle(q, points(mesh, id));
92                    let better = match best.get() {
93                        None => c.dist2 <= max2,
94                        Some((bid, b)) => c.dist2 < b.dist2 || (c.dist2 == b.dist2 && id < bid),
95                    };
96                    if better {
97                        best.set(Some((id, c)));
98                    }
99                }
100            }
101            true
102        },
103    );
104    best.get().map(|(triangle, c)| ClosestHit {
105        triangle,
106        point: c.point,
107        distance: c.dist2.sqrt(),
108        barycentric: c.barycentric,
109        feature: c.feature,
110    })
111}
112
113/// The point of the mesh nearest to `p` within `opts.max_distance`, or `None`
114/// when the mesh has no triangle that close. Equal distances go to the smallest
115/// triangle id. Triangles with zero area take part, as their edges.
116///
117/// # Errors
118/// [`QueryError::InvalidInput`] for a non-finite `p`, or a `max_distance` that
119/// is negative or NaN; then [`QueryError::MeshMismatch`] or
120/// [`QueryError::StaleBvh`] when `bvh` does not match the current `mesh`.
121pub fn closest_point(
122    mesh: &Mesh,
123    bvh: &Bvh,
124    p: [f32; 3],
125    opts: &ClosestOptions,
126) -> Result<Option<ClosestHit>> {
127    validate(p, opts)?;
128    check(mesh, bvh)?;
129    Ok(closest(mesh, bvh, p, opts))
130}
131
132/// The distance from `p` to a triangle, in `f64`: for measuring a result
133/// exactly when `f32` is not enough, as on long thin triangles, whose `f32`
134/// distances are dominated by rounding.
135pub fn distance_to_triangle(p: [f64; 3], corners: [[f64; 3]; 3]) -> f64 {
136    closest_on_triangle(p, corners).dist2.sqrt()
137}
138
139/// [`closest_point`] for many points: `out[i]` gets the result for `points[i]`.
140///
141/// # Errors
142/// [`QueryError::InvalidInput`] when the slices differ in length or any point
143/// is invalid, and otherwise as for [`closest_point`]. Everything is validated
144/// before anything is written, so on error `out` is unchanged.
145pub fn closest_point_many(
146    mesh: &Mesh,
147    bvh: &Bvh,
148    points: &[[f32; 3]],
149    opts: &ClosestOptions,
150    out: &mut [Option<ClosestHit>],
151) -> Result<()> {
152    if points.len() != out.len() {
153        return Err(QueryError::InvalidInput(
154            "points and out must have the same length",
155        ));
156    }
157    validate_opts(opts)?;
158    for &p in points {
159        validate_point(p)?;
160    }
161    check(mesh, bvh)?;
162    par::for_each_chunk_mut(out, QUERY_CHUNK, PAR_MIN_QUERIES, |i, slots| {
163        let points = &points[i * QUERY_CHUNK..];
164        for (&p, slot) in points.iter().zip(slots) {
165            *slot = closest(mesh, bvh, p, opts);
166        }
167    });
168    Ok(())
169}
170
171#[cfg(test)]
172mod tests {
173    use super::*;
174    use crate::test_util::{Lcg, build, wavy};
175    use crate::{Cull, Ray, RayOptions, raycast};
176    use fast_mesh_bvh::{Bvh, Quality};
177
178    const OPEN: ClosestOptions = ClosestOptions {
179        max_distance: f32::INFINITY,
180    };
181
182    /// The alive triangle with the smallest squared distance, ties to the smaller id.
183    fn brute(mesh: &Mesh, q: [f32; 3], max: f32) -> Option<(TriangleId, f32)> {
184        let mut best: Option<(TriangleId, f32)> = None;
185        for id in 0..mesh.triangle_slots() as TriangleId {
186            if !mesh.is_triangle_alive(id) {
187                continue;
188            }
189            let d2 = closest_on_triangle(q, points(mesh, id)).dist2;
190            if d2 <= max * max && best.is_none_or(|(_, b)| d2 < b) {
191                best = Some((id, d2));
192            }
193        }
194        best
195    }
196
197    /// 500 points around and above a `wavy(n)` grid.
198    fn queries(n: u32) -> Vec<[f32; 3]> {
199        let mut rng = Lcg(0xc105e57);
200        let span = n as f32 + 6.0;
201        (0..500)
202            .map(|_| {
203                [
204                    rng.next() * span - 3.0,
205                    rng.next() * span - 3.0,
206                    rng.next() * 6.0 - 1.0,
207                ]
208            })
209            .collect()
210    }
211
212    #[test]
213    fn matches_brute_force() {
214        let mesh = wavy(16);
215        for quality in [Quality::Sah, Quality::Fast] {
216            let bvh = build(&mesh, quality);
217            for q in queries(16) {
218                let got = closest_point(&mesh, &bvh, q, &OPEN).unwrap().unwrap();
219                let (id, d2) = brute(&mesh, q, f32::INFINITY).unwrap();
220                assert_eq!((got.triangle, got.distance), (id, d2.sqrt()), "{q:?}");
221                let c = closest_on_triangle(q, points(&mesh, id));
222                assert_eq!(
223                    (got.point, got.barycentric, got.feature),
224                    (c.point, c.barycentric, c.feature)
225                );
226            }
227        }
228    }
229
230    #[test]
231    fn max_distance_filters_like_brute_force() {
232        let mesh = wavy(16);
233        let bvh = build(&mesh, Quality::Sah);
234        for max_distance in [0.5, 1.5] {
235            let opts = ClosestOptions { max_distance };
236            let mut found = 0;
237            for q in queries(16) {
238                let got = closest_point(&mesh, &bvh, q, &opts)
239                    .unwrap()
240                    .map(|h| (h.triangle, h.distance));
241                let want = brute(&mesh, q, max_distance).map(|(id, d2)| (id, d2.sqrt()));
242                assert_eq!(got, want, "{q:?}");
243                found += usize::from(got.is_some());
244            }
245            assert!(found > 0);
246        }
247    }
248
249    #[test]
250    fn max_distance_is_inclusive() {
251        let flat =
252            Mesh::from_buffers(&[0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 4.0, 0.0], &[0, 1, 2]).unwrap();
253        let bvh = build(&flat, Quality::Sah);
254        let q = [1.0, 1.0, 2.0];
255        let at = |max_distance| ClosestOptions { max_distance };
256        let h = closest_point(&flat, &bvh, q, &at(2.0)).unwrap().unwrap();
257        assert_eq!(h.distance, 2.0);
258        assert_eq!(closest_point(&flat, &bvh, q, &at(1.999)), Ok(None));
259    }
260
261    #[test]
262    fn max_distance_zero_finds_only_surface_points() {
263        // Issue #19: with `max_distance = 0`, a query point on the surface is
264        // found, anything off it is not.
265        let flat =
266            Mesh::from_buffers(&[0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 4.0, 0.0], &[0, 1, 2]).unwrap();
267        let bvh = build(&flat, Quality::Sah);
268        let opts = ClosestOptions { max_distance: 0.0 };
269        let h = closest_point(&flat, &bvh, [1.0, 1.0, 0.0], &opts)
270            .unwrap()
271            .unwrap();
272        assert_eq!(h.distance, 0.0);
273        assert_eq!(h.point, [1.0, 1.0, 0.0]);
274        assert_eq!(
275            closest_point(&flat, &bvh, [1.0, 1.0, 1e-6], &opts),
276            Ok(None)
277        );
278        assert_eq!(closest_point(&flat, &bvh, [5.0, 5.0, 0.0], &opts), Ok(None));
279    }
280
281    #[test]
282    fn equal_distance_takes_smallest_id() {
283        // The point (0.5, 0.5, 0) lies on the diagonal shared by both triangles.
284        let p = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0];
285        let mesh = Mesh::from_buffers(&p, &[0, 2, 3, 0, 1, 2]).unwrap();
286        for quality in [Quality::Sah, Quality::Fast] {
287            let bvh = build(&mesh, quality);
288            let h = closest_point(&mesh, &bvh, [0.5, 0.5, 1.0], &OPEN)
289                .unwrap()
290                .unwrap();
291            assert_eq!(h.triangle, 0);
292        }
293    }
294
295    #[test]
296    fn degenerate_only_mesh() {
297        let p = [0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 2.0, 2.0, 0.0];
298        let mesh = Mesh::from_buffers(&p, &[0, 1, 2]).unwrap();
299        let bvh = build(&mesh, Quality::Sah);
300        let h = closest_point(&mesh, &bvh, [1.0, 0.0, 0.0], &OPEN)
301            .unwrap()
302            .unwrap();
303        assert_eq!(h.triangle, 0);
304        assert!(h.distance.is_finite());
305        assert_eq!(h.distance, 0.5f32.sqrt());
306        let ray = Ray {
307            origin: [1.0, 1.0, 1.0],
308            dir: [0.0, 0.0, -1.0],
309        };
310        let opts = RayOptions {
311            cull: Cull::None,
312            ..Default::default()
313        };
314        assert_eq!(raycast(&mesh, &bvh, &ray, &opts), Ok(None));
315    }
316
317    #[test]
318    fn empty_mesh_returns_nothing() {
319        let mesh = Mesh::new();
320        let bvh = build(&mesh, Quality::Sah);
321        assert_eq!(closest_point(&mesh, &bvh, [0.0; 3], &OPEN), Ok(None));
322        let mut out = [None];
323        closest_point_many(&mesh, &bvh, &[[0.0; 3]], &OPEN, &mut out).unwrap();
324        assert_eq!(out, [None]);
325    }
326
327    #[test]
328    fn invalid_input() {
329        let mesh = wavy(2);
330        let bvh = build(&mesh, Quality::Sah);
331        let invalid = |r: Result<Option<ClosestHit>>| matches!(r, Err(QueryError::InvalidInput(_)));
332        let nan = [f32::NAN, 0.0, 0.0];
333        assert!(invalid(closest_point(&mesh, &bvh, nan, &OPEN)));
334        for max_distance in [-1.0, f32::NAN] {
335            let opts = ClosestOptions { max_distance };
336            assert!(invalid(closest_point(&mesh, &bvh, [0.0; 3], &opts)));
337            let mut out = [None];
338            assert!(matches!(
339                closest_point_many(&mesh, &bvh, &[[0.0; 3]], &opts, &mut out),
340                Err(QueryError::InvalidInput(_))
341            ));
342        }
343        let mut out = [None, None];
344        assert!(matches!(
345            closest_point_many(&mesh, &bvh, &[[0.0; 3]], &OPEN, &mut out),
346            Err(QueryError::InvalidInput(_))
347        ));
348    }
349
350    #[test]
351    fn many_validates_before_writing() {
352        let mesh = wavy(2);
353        let bvh = build(&mesh, Quality::Sah);
354        let mut out = [None, None];
355        let r = closest_point_many(&mesh, &bvh, &[[0.0; 3], [f32::NAN; 3]], &OPEN, &mut out);
356        assert!(matches!(r, Err(QueryError::InvalidInput(_))));
357        assert_eq!(out, [None, None]);
358    }
359
360    #[test]
361    fn stale_and_mismatch_are_rejected() {
362        let mut mesh = wavy(2);
363        let bvh = build(&mesh, Quality::Sah);
364        let other = wavy(2);
365        assert_eq!(
366            closest_point(&other, &bvh, [0.0; 3], &OPEN),
367            Err(QueryError::MeshMismatch)
368        );
369        let mut tx = mesh.begin_edit();
370        tx.set_positions(&[0], &[[0.0, 0.0, 1.0]]).unwrap();
371        tx.commit().unwrap();
372        assert_eq!(
373            closest_point(&mesh, &bvh, [0.0; 3], &OPEN),
374            Err(QueryError::StaleBvh)
375        );
376        let mut out = [None];
377        assert_eq!(
378            closest_point_many(&mesh, &bvh, &[[0.0; 3]], &OPEN, &mut out),
379            Err(QueryError::StaleBvh)
380        );
381    }
382
383    #[test]
384    fn many_matches_single() {
385        let mesh = wavy(16);
386        let bvh: Bvh = build(&mesh, Quality::Sah);
387        let qs = queries(16);
388        let mut out = vec![None; qs.len()];
389        closest_point_many(&mesh, &bvh, &qs, &OPEN, &mut out).unwrap();
390        for (q, got) in qs.iter().zip(&out) {
391            assert_eq!(*got, closest_point(&mesh, &bvh, *q, &OPEN).unwrap());
392        }
393    }
394
395    #[test]
396    fn box_distance() {
397        use fast_mesh_bvh::Aabb;
398        let b = Aabb::new([0.0; 3], [1.0; 3]);
399        assert_eq!(box_dist2([0.5; 3], &b), 0.0);
400        assert_eq!(box_dist2([2.0, 0.5, 0.5], &b), 1.0);
401        assert_eq!(box_dist2([-1.0, 2.0, 0.5], &b), 2.0);
402    }
403
404    #[test]
405    fn batch_queries_bit_identical() {
406        use crate::raycast_many;
407        use crate::test_util::assert_parallel_identical;
408        let mesh = wavy(150);
409        let bvh = build(&mesh, Quality::Sah);
410        let mut rng = Lcg(0xba7c4);
411        let count = 20_000;
412        let span = 156.0;
413        let mut around = |z: f32| [rng.next() * span - 3.0, rng.next() * span - 3.0, z];
414        let points: Vec<[f32; 3]> = (0..count).map(|k| around((k % 7) as f32 - 1.0)).collect();
415        let origins: Vec<[f32; 3]> = (0..count).map(|_| around(5.0)).collect();
416        let dirs: Vec<[f32; 3]> = (0..count)
417            .map(|k| [(k % 5) as f32 * 0.2 - 0.4, (k % 3) as f32 * 0.3 - 0.3, -1.0])
418            .collect();
419        // Debug prints every float in full, so equal strings mean equal bits.
420        let closest = assert_parallel_identical("closest_point_many", || {
421            let mut out = vec![None; count];
422            closest_point_many(&mesh, &bvh, &points, &OPEN, &mut out).unwrap();
423            format!("{out:?}")
424        });
425        let one: Vec<_> = points
426            .iter()
427            .map(|&p| closest_point(&mesh, &bvh, p, &OPEN).unwrap())
428            .collect();
429        assert_eq!(closest, format!("{one:?}"));
430        let opts = RayOptions::default();
431        let hits = assert_parallel_identical("raycast_many", || {
432            let mut out = vec![None; count];
433            raycast_many(&mesh, &bvh, &origins, &dirs, &opts, &mut out).unwrap();
434            format!("{out:?}")
435        });
436        let one: Vec<_> = origins
437            .iter()
438            .zip(&dirs)
439            .map(|(&origin, &dir)| raycast(&mesh, &bvh, &Ray { origin, dir }, &opts).unwrap())
440            .collect();
441        assert_eq!(hits, format!("{one:?}"));
442        assert!(one.iter().filter(|h| h.is_some()).count() > count / 2);
443    }
444
445    #[test]
446    fn small_inputs_stay_sequential() {
447        use crate::{Rigid, intersecting_pairs, raycast_many, self_intersecting_pairs};
448        use crate::{test_util::crossed_ripples, triangles_near};
449        use fast_mesh_store::par;
450        let before = par::parallel_entries();
451        let mesh = wavy(20);
452        let bvh = build(&mesh, Quality::Sah);
453        let points = &queries(20)[..200];
454        let mut out = vec![None; points.len()];
455        closest_point_many(&mesh, &bvh, points, &OPEN, &mut out).unwrap();
456        let dirs = vec![[0.0, 0.0, -1.0]; points.len()];
457        let mut hits = vec![None; points.len()];
458        raycast_many(
459            &mesh,
460            &bvh,
461            points,
462            &dirs,
463            &RayOptions::default(),
464            &mut hits,
465        )
466        .unwrap();
467        let (mut near_a, mut near_b, mut pairs) = (Vec::new(), Vec::new(), Vec::new());
468        let id = Rigid::IDENTITY;
469        triangles_near(&mesh, &bvh, &mesh, &bvh, &id, 0.5, &mut near_a, &mut near_b).unwrap();
470        intersecting_pairs(&mesh, &bvh, &mesh, &bvh, &id, &mut pairs).unwrap();
471        assert!(!near_a.is_empty() && !pairs.is_empty());
472        let crossed = crossed_ripples(10);
473        let crossed_bvh = build(&crossed, Quality::Sah);
474        pairs.clear();
475        self_intersecting_pairs(&crossed, &crossed_bvh, &mut pairs).unwrap();
476        assert!(!pairs.is_empty());
477        assert_eq!(par::parallel_entries(), before);
478    }
479}
fast_mesh_query/closest.rs

fast_mesh_query/
closest.rs
