parry3d/bounding_volume/
simd_aabb.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Isometry, Point, Real, SimdBool, SimdReal, Vector, DIM, SIMD_WIDTH};
3use crate::query::SimdRay;
4use crate::utils::{self, IsometryOps};
5use num::{One, Zero};
6use simba::simd::{SimdPartialOrd, SimdValue};
7
8/// Four Aabb represented as a single SoA Aabb with SIMD components.
9#[derive(Debug, Copy, Clone)]
10#[cfg_attr(
11    feature = "rkyv",
12    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize),
13    archive(check_bytes)
14)]
15pub struct SimdAabb {
16    /// The min coordinates of the Aabbs.
17    pub mins: Point<SimdReal>,
18    /// The max coordinates the Aabbs.
19    pub maxs: Point<SimdReal>,
20}
21
22#[cfg(feature = "serde-serialize")]
23impl serde::Serialize for SimdAabb {
24    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
25    where
26        S: serde::Serializer,
27    {
28        use serde::ser::SerializeStruct;
29
30        let mins: Point<[Real; SIMD_WIDTH]> = Point::from(
31            self.mins
32                .coords
33                .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
34        );
35        let maxs: Point<[Real; SIMD_WIDTH]> = Point::from(
36            self.maxs
37                .coords
38                .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
39        );
40
41        let mut simd_aabb = serializer.serialize_struct("SimdAabb", 2)?;
42        simd_aabb.serialize_field("mins", &mins)?;
43        simd_aabb.serialize_field("maxs", &maxs)?;
44        simd_aabb.end()
45    }
46}
47
48#[cfg(feature = "serde-serialize")]
49impl<'de> serde::Deserialize<'de> for SimdAabb {
50    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
51    where
52        D: serde::Deserializer<'de>,
53    {
54        struct Visitor {}
55
56        #[derive(Deserialize)]
57        #[serde(field_identifier, rename_all = "lowercase")]
58        enum Field {
59            Mins,
60            Maxs,
61        }
62
63        impl<'de> serde::de::Visitor<'de> for Visitor {
64            type Value = SimdAabb;
65            fn expecting(&self, formatter: &mut core::fmt::Formatter) -> core::fmt::Result {
66                write!(
67                    formatter,
68                    "two arrays containing at least {} floats",
69                    SIMD_WIDTH * DIM * 2
70                )
71            }
72
73            fn visit_map<A>(self, mut map: A) -> Result<Self::Value, A::Error>
74            where
75                A: serde::de::MapAccess<'de>,
76            {
77                let mut mins: Option<Point<[Real; SIMD_WIDTH]>> = None;
78                let mut maxs: Option<Point<[Real; SIMD_WIDTH]>> = None;
79
80                while let Some(key) = map.next_key()? {
81                    match key {
82                        Field::Mins => {
83                            if mins.is_some() {
84                                return Err(serde::de::Error::duplicate_field("mins"));
85                            }
86                            mins = Some(map.next_value()?);
87                        }
88                        Field::Maxs => {
89                            if maxs.is_some() {
90                                return Err(serde::de::Error::duplicate_field("maxs"));
91                            }
92                            maxs = Some(map.next_value()?);
93                        }
94                    }
95                }
96
97                let mins = mins.ok_or_else(|| serde::de::Error::missing_field("mins"))?;
98                let maxs = maxs.ok_or_else(|| serde::de::Error::missing_field("maxs"))?;
99                let mins = mins.map(SimdReal::from);
100                let maxs = maxs.map(SimdReal::from);
101                Ok(SimdAabb { mins, maxs })
102            }
103
104            fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
105            where
106                A: serde::de::SeqAccess<'de>,
107            {
108                let mins: Point<[Real; SIMD_WIDTH]> = seq
109                    .next_element()?
110                    .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
111                let maxs: Point<[Real; SIMD_WIDTH]> = seq
112                    .next_element()?
113                    .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?;
114                let mins = mins.map(SimdReal::from);
115                let maxs = maxs.map(SimdReal::from);
116                Ok(SimdAabb { mins, maxs })
117            }
118        }
119
120        deserializer.deserialize_struct("SimdAabb", &["mins", "maxs"], Visitor {})
121    }
122}
123
124impl SimdAabb {
125    /// An invalid Aabb.
126    pub fn new_invalid() -> Self {
127        Self::splat(Aabb::new_invalid())
128    }
129
130    /// Builds an SIMD aabb composed of four identical aabbs.
131    pub fn splat(aabb: Aabb) -> Self {
132        Self {
133            mins: Point::splat(aabb.mins),
134            maxs: Point::splat(aabb.maxs),
135        }
136    }
137
138    /// The center of all the Aabbs represented by `self`.
139    pub fn center(&self) -> Point<SimdReal> {
140        na::center(&self.mins, &self.maxs)
141    }
142
143    /// The half-extents of all the Aabbs represented by `self`.
144    pub fn half_extents(&self) -> Vector<SimdReal> {
145        (self.maxs - self.mins) * SimdReal::splat(0.5)
146    }
147
148    /// The radius of all the Aabbs represented by `self`.
149    pub fn radius(&self) -> SimdReal {
150        (self.maxs - self.mins).norm()
151    }
152
153    /// Return the Aabb of the `self` transformed by the given isometry.
154    pub fn transform_by(&self, transform: &Isometry<SimdReal>) -> Self {
155        let ls_center = self.center();
156        let center = transform * ls_center;
157        let ws_half_extents = transform.absolute_transform_vector(&self.half_extents());
158        Self {
159            mins: center + (-ws_half_extents),
160            maxs: center + ws_half_extents,
161        }
162    }
163
164    /// Returns a scaled version of this Aabb.
165    #[inline]
166    pub fn scaled(self, scale: &Vector<SimdReal>) -> Self {
167        let a = self.mins.coords.component_mul(scale);
168        let b = self.maxs.coords.component_mul(scale);
169        Self {
170            mins: a.inf(&b).into(),
171            maxs: a.sup(&b).into(),
172        }
173    }
174
175    /// Enlarges this bounding volume by the given margin.
176    pub fn loosen(&mut self, margin: SimdReal) {
177        self.mins -= Vector::repeat(margin);
178        self.maxs += Vector::repeat(margin);
179    }
180
181    /// Dilate all the Aabbs represented by `self` by their extents multiplied
182    /// by the given scale `factor`.
183    pub fn dilate_by_factor(&mut self, factor: SimdReal) {
184        // If some of the Aabbs on this SimdAabb are invalid,
185        // don't, dilate them.
186        let is_valid = self.mins.x.simd_le(self.maxs.x);
187        let factor = factor.select(is_valid, SimdReal::zero());
188
189        // NOTE: we multiply each by factor instead of doing
190        // (maxs - mins) * factor. That's to avoid overflows (and
191        // therefore NaNs if this SimdAabb contains some invalid
192        // Aabbs initialised with Real::MAX
193        let dilation = self.maxs * factor - self.mins * factor;
194        self.mins -= dilation;
195        self.maxs += dilation;
196    }
197
198    /// Replace the `i-th` Aabb of this SIMD AAAB by the given value.
199    pub fn replace(&mut self, i: usize, aabb: Aabb) {
200        self.mins.replace(i, aabb.mins);
201        self.maxs.replace(i, aabb.maxs);
202    }
203
204    /// Casts a ray on all the Aabbs represented by `self`.
205    pub fn cast_local_ray(
206        &self,
207        ray: &SimdRay,
208        max_time_of_impact: SimdReal,
209    ) -> (SimdBool, SimdReal) {
210        let zero = SimdReal::zero();
211        let one = SimdReal::one();
212        let infinity = SimdReal::splat(Real::MAX);
213
214        let mut hit = SimdBool::splat(true);
215        let mut tmin = SimdReal::zero();
216        let mut tmax = max_time_of_impact;
217
218        // TODO: could this be optimized more considering we really just need a boolean answer?
219        for i in 0usize..DIM {
220            let is_not_zero = ray.dir[i].simd_ne(zero);
221            let is_zero_test =
222                ray.origin[i].simd_ge(self.mins[i]) & ray.origin[i].simd_le(self.maxs[i]);
223            let is_not_zero_test = {
224                let denom = one / ray.dir[i];
225                let mut inter_with_near_plane =
226                    ((self.mins[i] - ray.origin[i]) * denom).select(is_not_zero, -infinity);
227                let mut inter_with_far_plane =
228                    ((self.maxs[i] - ray.origin[i]) * denom).select(is_not_zero, infinity);
229
230                let gt = inter_with_near_plane.simd_gt(inter_with_far_plane);
231                utils::simd_swap(gt, &mut inter_with_near_plane, &mut inter_with_far_plane);
232
233                tmin = tmin.simd_max(inter_with_near_plane);
234                tmax = tmax.simd_min(inter_with_far_plane);
235
236                tmin.simd_le(tmax)
237            };
238
239            hit = hit & is_not_zero_test.select(is_not_zero, is_zero_test);
240        }
241
242        (hit, tmin)
243    }
244
245    /// Computes the distances between a point and all the Aabbs represented by `self`.
246    pub fn distance_to_local_point(&self, point: &Point<SimdReal>) -> SimdReal {
247        let mins_point = self.mins - point;
248        let point_maxs = point - self.maxs;
249        let shift = mins_point.sup(&point_maxs).sup(&na::zero());
250        shift.norm()
251    }
252
253    /// Computes the distances between the origin and all the Aabbs represented by `self`.
254    pub fn distance_to_origin(&self) -> SimdReal {
255        self.mins
256            .coords
257            .sup(&-self.maxs.coords)
258            .sup(&Vector::zeros())
259            .norm()
260    }
261
262    /// Check which Aabb represented by `self` contains the given `point`.
263    pub fn contains_local_point(&self, point: &Point<SimdReal>) -> SimdBool {
264        #[cfg(feature = "dim2")]
265        return self.mins.x.simd_le(point.x)
266            & self.mins.y.simd_le(point.y)
267            & self.maxs.x.simd_ge(point.x)
268            & self.maxs.y.simd_ge(point.y);
269
270        #[cfg(feature = "dim3")]
271        return self.mins.x.simd_le(point.x)
272            & self.mins.y.simd_le(point.y)
273            & self.mins.z.simd_le(point.z)
274            & self.maxs.x.simd_ge(point.x)
275            & self.maxs.y.simd_ge(point.y)
276            & self.maxs.z.simd_ge(point.z);
277    }
278
279    /// Lanewise check which Aabb represented by `self` contains the given set of `other` aabbs.
280    /// The check is performed lane-wise.
281    #[cfg(feature = "dim2")]
282    pub fn contains(&self, other: &SimdAabb) -> SimdBool {
283        self.mins.x.simd_le(other.mins.x)
284            & self.mins.y.simd_le(other.mins.y)
285            & self.maxs.x.simd_ge(other.maxs.x)
286            & self.maxs.y.simd_ge(other.maxs.y)
287    }
288
289    /// Lanewise check which Aabb represented by `self` contains the given set of `other` aabbs.
290    /// The check is performed lane-wise.
291    #[cfg(feature = "dim3")]
292    pub fn contains(&self, other: &SimdAabb) -> SimdBool {
293        self.mins.x.simd_le(other.mins.x)
294            & self.mins.y.simd_le(other.mins.y)
295            & self.mins.z.simd_le(other.mins.z)
296            & self.maxs.x.simd_ge(other.maxs.x)
297            & self.maxs.y.simd_ge(other.maxs.y)
298            & self.maxs.z.simd_ge(other.maxs.z)
299    }
300
301    /// Lanewise check which Aabb represented by `self` intersects the given set of `other` aabbs.
302    /// The check is performed lane-wise.
303    #[cfg(feature = "dim2")]
304    pub fn intersects(&self, other: &SimdAabb) -> SimdBool {
305        self.mins.x.simd_le(other.maxs.x)
306            & other.mins.x.simd_le(self.maxs.x)
307            & self.mins.y.simd_le(other.maxs.y)
308            & other.mins.y.simd_le(self.maxs.y)
309    }
310
311    /// Check which Aabb represented by `self` contains the given set of `other` aabbs.
312    /// The check is performed lane-wise.
313    #[cfg(feature = "dim3")]
314    pub fn intersects(&self, other: &SimdAabb) -> SimdBool {
315        self.mins.x.simd_le(other.maxs.x)
316            & other.mins.x.simd_le(self.maxs.x)
317            & self.mins.y.simd_le(other.maxs.y)
318            & other.mins.y.simd_le(self.maxs.y)
319            & self.mins.z.simd_le(other.maxs.z)
320            & other.mins.z.simd_le(self.maxs.z)
321    }
322
323    /// Checks intersections between all the lanes combination between `self` and `other`.
324    ///
325    /// The result is an array such that `result[i].extract(j)` contains the intersection
326    /// result between `self.extract(i)` and `other.extract(j)`.
327    pub fn intersects_permutations(&self, other: &SimdAabb) -> [SimdBool; SIMD_WIDTH] {
328        let mut result = [SimdBool::splat(false); SIMD_WIDTH];
329        for (ii, result) in result.iter_mut().enumerate() {
330            // TODO: use SIMD-accelerated shuffling?
331            let extracted = SimdAabb::splat(self.extract(ii));
332            *result = extracted.intersects(other);
333        }
334
335        result
336    }
337
338    /// Merge all the Aabb represented by `self` into a single one.
339    pub fn to_merged_aabb(&self) -> Aabb {
340        Aabb::new(
341            self.mins.coords.map(|e| e.simd_horizontal_min()).into(),
342            self.maxs.coords.map(|e| e.simd_horizontal_max()).into(),
343        )
344    }
345
346    /// Extracts the Aabb stored in the given SIMD lane of the SIMD Aabb:
347    pub fn extract(&self, lane: usize) -> Aabb {
348        Aabb::new(self.mins.extract(lane), self.maxs.extract(lane))
349    }
350}
351
352impl From<[Aabb; SIMD_WIDTH]> for SimdAabb {
353    fn from(aabbs: [Aabb; SIMD_WIDTH]) -> Self {
354        let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH];
355        let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH];
356
357        SimdAabb {
358            mins: Point::from(mins),
359            maxs: Point::from(maxs),
360        }
361    }
362}
parry3d/bounding_volume/simd_aabb.rs

parry3d/bounding_volume/
simd_aabb.rs
