parry3d_f64/query/distance/
distance_composite_shape_shape.rs

1use crate::bounding_volume::SimdAabb;
2use crate::math::{Isometry, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH};
3use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor};
4use crate::query::QueryDispatcher;
5use crate::shape::{Shape, TypedSimdCompositeShape};
6use crate::utils::IsometryOpt;
7use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
8
9/// Smallest distance between a composite shape and any other shape.
10pub fn distance_composite_shape_shape<D, G1>(
11    dispatcher: &D,
12    pos12: &Isometry<Real>,
13    g1: &G1,
14    g2: &dyn Shape,
15) -> Real
16where
17    D: ?Sized + QueryDispatcher,
18    G1: ?Sized + TypedSimdCompositeShape,
19{
20    let mut visitor = CompositeShapeAgainstAnyDistanceVisitor::new(dispatcher, pos12, g1, g2);
21    g1.typed_qbvh()
22        .traverse_best_first(&mut visitor)
23        .expect("The composite shape must not be empty.")
24        .1
25         .1
26}
27
28/// Smallest distance between a shape and a composite shape.
29pub fn distance_shape_composite_shape<D, G2>(
30    dispatcher: &D,
31    pos12: &Isometry<Real>,
32    g1: &dyn Shape,
33    g2: &G2,
34) -> Real
35where
36    D: ?Sized + QueryDispatcher,
37    G2: ?Sized + TypedSimdCompositeShape,
38{
39    distance_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1)
40}
41
42/// A visitor for computing the distance between a composite shape and a shape.
43pub struct CompositeShapeAgainstAnyDistanceVisitor<'a, D: ?Sized, G1: ?Sized + 'a> {
44    msum_shift: Vector<SimdReal>,
45    msum_margin: Vector<SimdReal>,
46
47    dispatcher: &'a D,
48    pos12: &'a Isometry<Real>,
49    g1: &'a G1,
50    g2: &'a dyn Shape,
51}
52
53impl<'a, D: ?Sized, G1: ?Sized + 'a> CompositeShapeAgainstAnyDistanceVisitor<'a, D, G1> {
54    /// Initialize a visitor for computing the distance between a composite shape and a shape.
55    pub fn new(
56        dispatcher: &'a D,
57        pos12: &'a Isometry<Real>,
58        g1: &'a G1,
59        g2: &'a dyn Shape,
60    ) -> Self {
61        let ls_aabb2 = g2.compute_aabb(pos12);
62
63        Self {
64            dispatcher,
65            msum_shift: Vector::splat(-ls_aabb2.center().coords),
66            msum_margin: Vector::splat(ls_aabb2.half_extents()),
67            pos12,
68            g1,
69            g2,
70        }
71    }
72}
73
74impl<D, G1> SimdBestFirstVisitor<G1::PartId, SimdAabb>
75    for CompositeShapeAgainstAnyDistanceVisitor<'_, D, G1>
76where
77    D: ?Sized + QueryDispatcher,
78    G1: ?Sized + TypedSimdCompositeShape,
79{
80    type Result = (G1::PartId, Real);
81
82    fn visit(
83        &mut self,
84        best: Real,
85        bv: &SimdAabb,
86        data: Option<[Option<&G1::PartId>; SIMD_WIDTH]>,
87    ) -> SimdBestFirstVisitStatus<Self::Result> {
88        // Compute the minkowski sum of the two Aabbs.
89        let msum = SimdAabb {
90            mins: bv.mins + self.msum_shift + (-self.msum_margin),
91            maxs: bv.maxs + self.msum_shift + self.msum_margin,
92        };
93        let dist = msum.distance_to_origin();
94        let mask = dist.simd_lt(SimdReal::splat(best));
95
96        if let Some(data) = data {
97            let bitmask = mask.bitmask();
98            let mut weights = [0.0; SIMD_WIDTH];
99            let mut mask = [false; SIMD_WIDTH];
100            let mut results = [None; SIMD_WIDTH];
101
102            for ii in 0..SIMD_WIDTH {
103                if (bitmask & (1 << ii)) != 0 && data[ii].is_some() {
104                    let part_id = *data[ii].unwrap();
105                    let mut dist = Ok(0.0);
106                    self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| {
107                        dist =
108                            self.dispatcher
109                                .distance(&part_pos1.inv_mul(self.pos12), g1, self.g2);
110                    });
111
112                    if let Ok(dist) = dist {
113                        if dist == 0.0 {
114                            return SimdBestFirstVisitStatus::ExitEarly(Some((part_id, 0.0)));
115                        } else {
116                            weights[ii] = dist;
117                            mask[ii] = dist < best;
118                            results[ii] = Some((part_id, dist));
119                        }
120                    }
121                }
122            }
123
124            SimdBestFirstVisitStatus::MaybeContinue {
125                weights: SimdReal::from(weights),
126                mask: SimdBool::from(mask),
127                results,
128            }
129        } else {
130            SimdBestFirstVisitStatus::MaybeContinue {
131                weights: dist,
132                mask,
133                results: [None; SIMD_WIDTH],
134            }
135        }
136    }
137}
parry3d_f64/query/distance/distance_composite_shape_shape.rs

parry3d_f64/query/distance/
distance_composite_shape_shape.rs
