parry3d_f64/query/shape_cast/
shape_cast_composite_shape_shape.rs

1use crate::bounding_volume::SimdAabb;
2use crate::math::{Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH};
3use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor};
4use crate::query::shape_cast::ShapeCastOptions;
5use crate::query::{QueryDispatcher, Ray, ShapeCastHit, SimdRay};
6use crate::shape::{Shape, TypedSimdCompositeShape};
7use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
8
9/// Time Of Impact of a composite shape with any other shape, under translational movement.
10pub fn cast_shapes_composite_shape_shape<D, G1>(
11    dispatcher: &D,
12    pos12: &Isometry<Real>,
13    vel12: &Vector<Real>,
14    g1: &G1,
15    g2: &dyn Shape,
16    options: ShapeCastOptions,
17) -> Option<ShapeCastHit>
18where
19    D: ?Sized + QueryDispatcher,
20    G1: ?Sized + TypedSimdCompositeShape,
21{
22    let mut visitor =
23        TOICompositeShapeShapeBestFirstVisitor::new(dispatcher, pos12, vel12, g1, g2, options);
24    g1.typed_qbvh()
25        .traverse_best_first(&mut visitor)
26        .map(|res| res.1 .1)
27}
28
29/// Time Of Impact of any shape with a composite shape, under translational movement.
30pub fn cast_shapes_shape_composite_shape<D, G2>(
31    dispatcher: &D,
32    pos12: &Isometry<Real>,
33    vel12: &Vector<Real>,
34    g1: &dyn Shape,
35    g2: &G2,
36    options: ShapeCastOptions,
37) -> Option<ShapeCastHit>
38where
39    D: ?Sized + QueryDispatcher,
40    G2: ?Sized + TypedSimdCompositeShape,
41{
42    cast_shapes_composite_shape_shape(
43        dispatcher,
44        &pos12.inverse(),
45        &-pos12.inverse_transform_vector(vel12),
46        g2,
47        g1,
48        options,
49    )
50    .map(|time_of_impact| time_of_impact.swapped())
51}
52
53/// A visitor used to find the time-of-impact between a composite shape and a shape.
54pub struct TOICompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?Sized + 'a> {
55    msum_shift: Vector<SimdReal>,
56    msum_margin: Vector<SimdReal>,
57    ray: SimdRay,
58
59    dispatcher: &'a D,
60    pos12: &'a Isometry<Real>,
61    vel12: &'a Vector<Real>,
62    g1: &'a G1,
63    g2: &'a dyn Shape,
64    options: ShapeCastOptions,
65}
66
67impl<'a, D, G1> TOICompositeShapeShapeBestFirstVisitor<'a, D, G1>
68where
69    D: ?Sized + QueryDispatcher,
70    G1: ?Sized + TypedSimdCompositeShape,
71{
72    /// Creates a new visitor used to find the time-of-impact between a composite shape and a shape.
73    pub fn new(
74        dispatcher: &'a D,
75        pos12: &'a Isometry<Real>,
76        vel12: &'a Vector<Real>,
77        g1: &'a G1,
78        g2: &'a dyn Shape,
79        options: ShapeCastOptions,
80    ) -> TOICompositeShapeShapeBestFirstVisitor<'a, D, G1> {
81        let ls_aabb2 = g2.compute_aabb(pos12);
82        let ray = Ray::new(Point::origin(), *vel12);
83
84        TOICompositeShapeShapeBestFirstVisitor {
85            dispatcher,
86            msum_shift: Vector::splat(-ls_aabb2.center().coords),
87            msum_margin: Vector::splat(
88                ls_aabb2.half_extents() + Vector::repeat(options.target_distance),
89            ),
90            ray: SimdRay::splat(ray),
91            pos12,
92            vel12,
93            g1,
94            g2,
95            options,
96        }
97    }
98}
99
100impl<D, G1> SimdBestFirstVisitor<G1::PartId, SimdAabb>
101    for TOICompositeShapeShapeBestFirstVisitor<'_, D, G1>
102where
103    D: ?Sized + QueryDispatcher,
104    G1: ?Sized + TypedSimdCompositeShape,
105{
106    type Result = (G1::PartId, ShapeCastHit);
107
108    #[inline]
109    fn visit(
110        &mut self,
111        best: Real,
112        bv: &SimdAabb,
113        data: Option<[Option<&G1::PartId>; SIMD_WIDTH]>,
114    ) -> SimdBestFirstVisitStatus<Self::Result> {
115        // Compute the minkowski sum of the two Aabbs.
116        let msum = SimdAabb {
117            mins: bv.mins + self.msum_shift + (-self.msum_margin),
118            maxs: bv.maxs + self.msum_shift + self.msum_margin,
119        };
120
121        // Compute the time of impact.
122        let (mask, time_of_impact) =
123            msum.cast_local_ray(&self.ray, SimdReal::splat(self.options.max_time_of_impact));
124
125        if let Some(data) = data {
126            let better_toi = time_of_impact.simd_lt(SimdReal::splat(best));
127            let bitmask = (mask & better_toi).bitmask();
128            let mut weights = [0.0; SIMD_WIDTH];
129            let mut mask = [false; SIMD_WIDTH];
130            let mut results = [None; SIMD_WIDTH];
131
132            for ii in 0..SIMD_WIDTH {
133                if (bitmask & (1 << ii)) != 0 && data[ii].is_some() {
134                    let part_id = *data[ii].unwrap();
135                    let mut hit = None;
136                    self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| {
137                        if let Some(part_pos1) = part_pos1 {
138                            hit = self
139                                .dispatcher
140                                .cast_shapes(
141                                    &part_pos1.inv_mul(self.pos12),
142                                    &part_pos1.inverse_transform_vector(self.vel12),
143                                    g1,
144                                    self.g2,
145                                    self.options,
146                                )
147                                .unwrap_or(None)
148                                .map(|hit| hit.transform1_by(part_pos1));
149                        } else {
150                            hit = self
151                                .dispatcher
152                                .cast_shapes(self.pos12, self.vel12, g1, self.g2, self.options)
153                                .unwrap_or(None);
154                        }
155                    });
156
157                    if let Some(hit) = hit {
158                        results[ii] = Some((part_id, hit));
159                        mask[ii] = hit.time_of_impact < best;
160                        weights[ii] = hit.time_of_impact;
161                    }
162                }
163            }
164
165            SimdBestFirstVisitStatus::MaybeContinue {
166                weights: SimdReal::from(weights),
167                mask: SimdBool::from(mask),
168                results,
169            }
170        } else {
171            SimdBestFirstVisitStatus::MaybeContinue {
172                weights: time_of_impact,
173                mask,
174                results: [None; SIMD_WIDTH],
175            }
176        }
177    }
178}
parry3d_f64/query/shape_cast/shape_cast_composite_shape_shape.rs

parry3d_f64/query/shape_cast/
shape_cast_composite_shape_shape.rs
