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

parry3d_f64/query/closest_points/
closest_points_composite_shape_shape.rs
