parry3d_f64/query/visitors/
bounding_volume_intersections_simultaneous_visitor.rs

1use crate::bounding_volume::SimdAabb;
2use crate::math::{Isometry, Real, SimdReal, SIMD_WIDTH};
3use crate::partitioning::{SimdSimultaneousVisitStatus, SimdSimultaneousVisitor};
4use core::marker::PhantomData;
5use na::SimdValue;
6use simba::simd::SimdBool as _;
7
8#[cfg(feature = "parallel")]
9use crate::partitioning::{QbvhNode, SimdNodeIndex};
10
11/// Spatial partitioning data structure visitor collecting interferences with a given bounding volume.
12pub struct BoundingVolumeIntersectionsSimultaneousVisitor<T1, T2, F> {
13    pos12: Option<Isometry<SimdReal>>,
14    callback: F,
15    _phantom: PhantomData<(T1, T2)>,
16}
17
18impl<T1, T2, F> BoundingVolumeIntersectionsSimultaneousVisitor<T1, T2, F> {
19    /// Creates a new `BoundingVolumeIntersectionsSimultaneousVisitor`.
20    #[inline]
21    pub fn new(callback: F) -> BoundingVolumeIntersectionsSimultaneousVisitor<T1, T2, F> {
22        BoundingVolumeIntersectionsSimultaneousVisitor {
23            pos12: None,
24            callback,
25            _phantom: PhantomData,
26        }
27    }
28
29    /// Creates a new `BoundingVolumeIntersectionsSimultaneousVisitor`.
30    #[inline]
31    pub fn with_relative_pos(
32        pos12: Isometry<Real>,
33        callback: F,
34    ) -> BoundingVolumeIntersectionsSimultaneousVisitor<T1, T2, F> {
35        BoundingVolumeIntersectionsSimultaneousVisitor {
36            pos12: Some(Isometry::splat(pos12)),
37            callback,
38            _phantom: PhantomData,
39        }
40    }
41}
42
43impl<T1, T2, F> SimdSimultaneousVisitor<T1, T2, SimdAabb>
44    for BoundingVolumeIntersectionsSimultaneousVisitor<T1, T2, F>
45where
46    F: FnMut(&T1, &T2) -> bool,
47{
48    #[inline]
49    fn visit(
50        &mut self,
51        left_bv: &SimdAabb,
52        left_data: Option<[Option<&T1>; SIMD_WIDTH]>,
53        right_bv: &SimdAabb,
54        right_data: Option<[Option<&T2>; SIMD_WIDTH]>,
55    ) -> SimdSimultaneousVisitStatus {
56        let mask = if let Some(pos12) = &self.pos12 {
57            let transformed_right_bv = right_bv.transform_by(pos12);
58            left_bv.intersects_permutations(&transformed_right_bv)
59        } else {
60            left_bv.intersects_permutations(right_bv)
61        };
62
63        if let (Some(data1), Some(data2)) = (left_data, right_data) {
64            for (ii, data1) in data1.into_iter().enumerate() {
65                let Some(data1) = data1 else { continue };
66                let bitmask = mask[ii].bitmask();
67
68                for (jj, data2) in data2.into_iter().enumerate() {
69                    let Some(data2) = data2 else { continue };
70                    if (bitmask & (1 << jj)) != 0 && !(self.callback)(data1, data2) {
71                        return SimdSimultaneousVisitStatus::ExitEarly;
72                    }
73                }
74            }
75        }
76
77        SimdSimultaneousVisitStatus::MaybeContinue(mask)
78    }
79}
80
81#[cfg(feature = "parallel")]
82impl<LeafData1: Sync, LeafData2: Sync, F>
83    crate::partitioning::ParallelSimdSimultaneousVisitor<LeafData1, LeafData2>
84    for BoundingVolumeIntersectionsSimultaneousVisitor<LeafData1, LeafData2, F>
85where
86    F: Sync + Fn(&LeafData1, &LeafData2) -> bool,
87{
88    type Data = ();
89
90    #[inline]
91    fn visit(
92        &self,
93        _: SimdNodeIndex,
94        left_node: &QbvhNode,
95        left_data: Option<[Option<&LeafData1>; SIMD_WIDTH]>,
96        _: SimdNodeIndex,
97        right_node: &QbvhNode,
98        right_data: Option<[Option<&LeafData2>; SIMD_WIDTH]>,
99        _: (),
100    ) -> (SimdSimultaneousVisitStatus, ()) {
101        let mask = if let Some(pos12) = &self.pos12 {
102            let transformed_right_bv = right_node.simd_aabb.transform_by(pos12);
103            left_node
104                .simd_aabb
105                .intersects_permutations(&transformed_right_bv)
106        } else {
107            left_node
108                .simd_aabb
109                .intersects_permutations(&right_node.simd_aabb)
110        };
111
112        if let (Some(data1), Some(data2)) = (left_data, right_data) {
113            for (ii, data1) in data1.into_iter().enumerate() {
114                let Some(data1) = data1 else { continue };
115                let bitmask = mask[ii].bitmask();
116
117                for (jj, data2) in data2.into_iter().enumerate() {
118                    let Some(data2) = data2 else { continue };
119                    if (bitmask & (1 << jj)) != 0 {
120                        if !(self.callback)(data1, data2) {
121                            return (SimdSimultaneousVisitStatus::ExitEarly, ());
122                        }
123                    }
124                }
125            }
126        }
127
128        (SimdSimultaneousVisitStatus::MaybeContinue(mask), ())
129    }
130}
parry3d_f64/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs

parry3d_f64/query/visitors/
bounding_volume_intersections_simultaneous_visitor.rs
