parry3d_f64/query/visitors/
point_intersections_visitor.rs

1use crate::bounding_volume::SimdAabb;
2use crate::math::{Point, Real, SimdReal, SIMD_WIDTH};
3use crate::partitioning::{SimdVisitStatus, SimdVisitor};
4use core::marker::PhantomData;
5use simba::simd::{SimdBool as _, SimdValue};
6
7// TODO: add a point cost fn.
8
9/// Spatial partitioning structure visitor collecting nodes that may contain a given point.
10pub struct PointIntersectionsVisitor<'a, T, F> {
11    simd_point: Point<SimdReal>,
12    /// Callback executed for each leaf which Aabb contains `self.point`.
13    callback: &'a mut F,
14    _phantom: PhantomData<T>,
15}
16
17impl<'a, T, F> PointIntersectionsVisitor<'a, T, F>
18where
19    F: FnMut(&T) -> bool,
20{
21    /// Creates a new `PointIntersectionsVisitor`.
22    #[inline]
23    pub fn new(point: &'a Point<Real>, callback: &'a mut F) -> PointIntersectionsVisitor<'a, T, F> {
24        PointIntersectionsVisitor {
25            simd_point: Point::splat(*point),
26            callback,
27            _phantom: PhantomData,
28        }
29    }
30}
31
32impl<T, F> SimdVisitor<T, SimdAabb> for PointIntersectionsVisitor<'_, T, F>
33where
34    F: FnMut(&T) -> bool,
35{
36    #[inline]
37    fn visit(&mut self, bv: &SimdAabb, b: Option<[Option<&T>; SIMD_WIDTH]>) -> SimdVisitStatus {
38        let mask = bv.contains_local_point(&self.simd_point);
39
40        if let Some(data) = b {
41            let bitmask = mask.bitmask();
42
43            for (ii, data) in data.iter().enumerate() {
44                if (bitmask & (1 << ii)) != 0 {
45                    let Some(data) = data else { continue };
46                    if !(self.callback)(data) {
47                        return SimdVisitStatus::ExitEarly;
48                    }
49                }
50            }
51        }
52
53        SimdVisitStatus::MaybeContinue(mask)
54    }
55}
parry3d_f64/query/visitors/point_intersections_visitor.rs

parry3d_f64/query/visitors/
point_intersections_visitor.rs
