parry3d_f64/query/ray/
ray_composite_shape.rs

1use crate::bounding_volume::SimdAabb;
2use crate::math::{Real, SimdBool, SimdReal, SIMD_WIDTH};
3use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor};
4use crate::query::{Ray, RayCast, RayIntersection, SimdRay};
5use crate::shape::{Compound, FeatureId, Polyline, TriMesh, TypedSimdCompositeShape};
6use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
7
8impl RayCast for TriMesh {
9    #[inline]
10    fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option<Real> {
11        let mut visitor =
12            RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_time_of_impact, solid);
13
14        self.qbvh()
15            .traverse_best_first(&mut visitor)
16            .map(|res| res.1 .1)
17    }
18
19    #[inline]
20    fn cast_local_ray_and_get_normal(
21        &self,
22        ray: &Ray,
23        max_time_of_impact: Real,
24        solid: bool,
25    ) -> Option<RayIntersection> {
26        let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
27            self,
28            ray,
29            max_time_of_impact,
30            solid,
31        );
32
33        self.qbvh()
34            .traverse_best_first(&mut visitor)
35            .map(|(_, (best, mut res))| {
36                // We hit a backface.
37                // NOTE: we need this for `TriMesh::is_backface` to work properly.
38                if res.feature == FeatureId::Face(1) {
39                    res.feature = FeatureId::Face(best + self.indices().len() as u32)
40                } else {
41                    res.feature = FeatureId::Face(best);
42                }
43                res
44            })
45    }
46}
47
48impl RayCast for Polyline {
49    #[inline]
50    fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option<Real> {
51        let mut visitor =
52            RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_time_of_impact, solid);
53
54        self.qbvh()
55            .traverse_best_first(&mut visitor)
56            .map(|res| res.1 .1)
57    }
58
59    #[inline]
60    fn cast_local_ray_and_get_normal(
61        &self,
62        ray: &Ray,
63        max_time_of_impact: Real,
64        solid: bool,
65    ) -> Option<RayIntersection> {
66        let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
67            self,
68            ray,
69            max_time_of_impact,
70            solid,
71        );
72
73        self.qbvh()
74            .traverse_best_first(&mut visitor)
75            .map(|(_, (_, res))| res)
76    }
77}
78
79impl RayCast for Compound {
80    #[inline]
81    fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option<Real> {
82        let mut visitor =
83            RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_time_of_impact, solid);
84
85        self.qbvh()
86            .traverse_best_first(&mut visitor)
87            .map(|res| res.1 .1)
88    }
89
90    #[inline]
91    fn cast_local_ray_and_get_normal(
92        &self,
93        ray: &Ray,
94        max_time_of_impact: Real,
95        solid: bool,
96    ) -> Option<RayIntersection> {
97        let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
98            self,
99            ray,
100            max_time_of_impact,
101            solid,
102        );
103
104        self.qbvh()
105            .traverse_best_first(&mut visitor)
106            .map(|(_, (_, res))| res)
107    }
108}
109
110/*
111 * Visitors
112 */
113/// A visitor for casting a ray on a composite shape.
114pub struct RayCompositeShapeToiBestFirstVisitor<'a, S> {
115    shape: &'a S,
116    ray: &'a Ray,
117    simd_ray: SimdRay,
118    max_time_of_impact: Real,
119    solid: bool,
120}
121
122impl<'a, S> RayCompositeShapeToiBestFirstVisitor<'a, S> {
123    /// Initialize a visitor for casting a ray on a composite shape.
124    pub fn new(shape: &'a S, ray: &'a Ray, max_time_of_impact: Real, solid: bool) -> Self {
125        Self {
126            shape,
127            ray,
128            simd_ray: SimdRay::splat(*ray),
129            max_time_of_impact,
130            solid,
131        }
132    }
133}
134
135impl<S> SimdBestFirstVisitor<S::PartId, SimdAabb> for RayCompositeShapeToiBestFirstVisitor<'_, S>
136where
137    S: TypedSimdCompositeShape,
138{
139    type Result = (S::PartId, Real);
140
141    #[inline]
142    fn visit(
143        &mut self,
144        best: Real,
145        aabb: &SimdAabb,
146        data: Option<[Option<&S::PartId>; SIMD_WIDTH]>,
147    ) -> SimdBestFirstVisitStatus<Self::Result> {
148        let (hit, time_of_impact) =
149            aabb.cast_local_ray(&self.simd_ray, SimdReal::splat(self.max_time_of_impact));
150
151        if let Some(data) = data {
152            let mut weights = [0.0; SIMD_WIDTH];
153            let mut mask = [false; SIMD_WIDTH];
154            let mut results = [None; SIMD_WIDTH];
155
156            let better_toi = time_of_impact.simd_lt(SimdReal::splat(best));
157            let bitmask = (hit & better_toi).bitmask();
158
159            for ii in 0..SIMD_WIDTH {
160                if (bitmask & (1 << ii)) != 0 && data[ii].is_some() {
161                    let part_id = *data[ii].unwrap();
162                    self.shape
163                        .map_typed_part_at(part_id, |part_pos, part_shape, _| {
164                            let time_of_impact = if let Some(part_pos) = part_pos {
165                                part_shape.cast_ray(
166                                    part_pos,
167                                    self.ray,
168                                    self.max_time_of_impact,
169                                    self.solid,
170                                )
171                            } else {
172                                part_shape.cast_local_ray(
173                                    self.ray,
174                                    self.max_time_of_impact,
175                                    self.solid,
176                                )
177                            };
178                            if let Some(time_of_impact) = time_of_impact {
179                                results[ii] = Some((part_id, time_of_impact));
180                                mask[ii] = true;
181                                weights[ii] = time_of_impact;
182                            }
183                        })
184                }
185            }
186
187            SimdBestFirstVisitStatus::MaybeContinue {
188                weights: SimdReal::from(weights),
189                mask: SimdBool::from(mask),
190                results,
191            }
192        } else {
193            SimdBestFirstVisitStatus::MaybeContinue {
194                weights: time_of_impact,
195                mask: hit,
196                results: [None; SIMD_WIDTH],
197            }
198        }
199    }
200}
201
202/// A visitor for casting a ray on a composite shape.
203pub struct RayCompositeShapeToiAndNormalBestFirstVisitor<'a, S> {
204    shape: &'a S,
205    ray: &'a Ray,
206    simd_ray: SimdRay,
207    max_time_of_impact: Real,
208    solid: bool,
209}
210
211impl<'a, S> RayCompositeShapeToiAndNormalBestFirstVisitor<'a, S> {
212    /// Initialize a visitor for casting a ray on a composite shape.
213    pub fn new(shape: &'a S, ray: &'a Ray, max_time_of_impact: Real, solid: bool) -> Self {
214        Self {
215            shape,
216            ray,
217            simd_ray: SimdRay::splat(*ray),
218            max_time_of_impact,
219            solid,
220        }
221    }
222}
223
224impl<S> SimdBestFirstVisitor<S::PartId, SimdAabb>
225    for RayCompositeShapeToiAndNormalBestFirstVisitor<'_, S>
226where
227    S: TypedSimdCompositeShape,
228{
229    type Result = (S::PartId, RayIntersection);
230
231    #[inline]
232    fn visit(
233        &mut self,
234        best: Real,
235        aabb: &SimdAabb,
236        data: Option<[Option<&S::PartId>; SIMD_WIDTH]>,
237    ) -> SimdBestFirstVisitStatus<Self::Result> {
238        let (hit, time_of_impact) =
239            aabb.cast_local_ray(&self.simd_ray, SimdReal::splat(self.max_time_of_impact));
240
241        if let Some(data) = data {
242            let mut weights = [0.0; SIMD_WIDTH];
243            let mut mask = [false; SIMD_WIDTH];
244            let mut results = [None; SIMD_WIDTH];
245
246            let better_toi = time_of_impact.simd_lt(SimdReal::splat(best));
247            let bitmask = (hit & better_toi).bitmask();
248
249            for ii in 0..SIMD_WIDTH {
250                if (bitmask & (1 << ii)) != 0 && data[ii].is_some() {
251                    self.shape
252                        .map_typed_part_at(*data[ii].unwrap(), |part_pos, part_shape, _| {
253                            let result = if let Some(part_pos) = part_pos {
254                                part_shape.cast_ray_and_get_normal(
255                                    part_pos,
256                                    self.ray,
257                                    self.max_time_of_impact,
258                                    self.solid,
259                                )
260                            } else {
261                                part_shape.cast_local_ray_and_get_normal(
262                                    self.ray,
263                                    self.max_time_of_impact,
264                                    self.solid,
265                                )
266                            };
267
268                            if let Some(result) = result {
269                                results[ii] = Some((*data[ii].unwrap(), result));
270                                mask[ii] = true;
271                                weights[ii] = result.time_of_impact;
272                            }
273                        });
274                }
275            }
276
277            SimdBestFirstVisitStatus::MaybeContinue {
278                weights: SimdReal::from(weights),
279                mask: SimdBool::from(mask),
280                results,
281            }
282        } else {
283            SimdBestFirstVisitStatus::MaybeContinue {
284                weights: time_of_impact,
285                mask: hit,
286                results: [None; SIMD_WIDTH],
287            }
288        }
289    }
290}
parry3d_f64/query/ray/ray_composite_shape.rs

parry3d_f64/query/ray/
ray_composite_shape.rs
