parry3d_f64/query/intersection_test/
intersection_test_composite_shape_shape.rs

1use crate::bounding_volume::SimdAabb;
2use crate::math::{Isometry, Real, SIMD_WIDTH};
3use crate::partitioning::{SimdVisitStatus, SimdVisitor};
4use crate::query::QueryDispatcher;
5use crate::shape::{Shape, TypedSimdCompositeShape};
6use crate::utils::IsometryOpt;
7use simba::simd::SimdBool as _;
8
9/// Intersection test between a composite shape (`Mesh`, `Compound`) and any other shape.
10pub fn intersection_test_composite_shape_shape<D, G1>(
11    dispatcher: &D,
12    pos12: &Isometry<Real>,
13    g1: &G1,
14    g2: &dyn Shape,
15) -> bool
16where
17    D: ?Sized + QueryDispatcher,
18    G1: ?Sized + TypedSimdCompositeShape,
19{
20    let mut visitor = IntersectionCompositeShapeShapeVisitor::new(dispatcher, pos12, g1, g2);
21
22    let _ = g1.typed_qbvh().traverse_depth_first(&mut visitor);
23    visitor.found_intersection.is_some()
24}
25
26/// Proximity between a shape and a composite (`Mesh`, `Compound`) shape.
27pub fn intersection_test_shape_composite_shape<D, G2>(
28    dispatcher: &D,
29    pos12: &Isometry<Real>,
30    g1: &dyn Shape,
31    g2: &G2,
32) -> bool
33where
34    D: ?Sized + QueryDispatcher,
35    G2: ?Sized + TypedSimdCompositeShape,
36{
37    intersection_test_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1)
38}
39
40/// A visitor for checking if a composite-shape and a shape intersect.
41pub struct IntersectionCompositeShapeShapeVisitor<'a, D: ?Sized, G1: 'a>
42where
43    G1: ?Sized + TypedSimdCompositeShape,
44{
45    ls_aabb2: SimdAabb,
46
47    dispatcher: &'a D,
48    pos12: &'a Isometry<Real>,
49    g1: &'a G1,
50    g2: &'a dyn Shape,
51
52    /// Populated after the traversal.
53    ///
54    /// Is [`None`] if no intersection was found.
55    pub found_intersection: Option<G1::PartId>,
56}
57
58impl<'a, D, G1> IntersectionCompositeShapeShapeVisitor<'a, D, G1>
59where
60    D: ?Sized + QueryDispatcher,
61    G1: ?Sized + TypedSimdCompositeShape,
62{
63    /// Initialize a visitor for checking if a composite-shape and a shape intersect.
64    pub fn new(
65        dispatcher: &'a D,
66        pos12: &'a Isometry<Real>,
67        g1: &'a G1,
68        g2: &'a dyn Shape,
69    ) -> IntersectionCompositeShapeShapeVisitor<'a, D, G1> {
70        let ls_aabb2 = g2.compute_aabb(pos12);
71
72        IntersectionCompositeShapeShapeVisitor {
73            dispatcher,
74            ls_aabb2: SimdAabb::splat(ls_aabb2),
75            pos12,
76            g1,
77            g2,
78            found_intersection: None,
79        }
80    }
81}
82
83impl<D, G1> SimdVisitor<G1::PartId, SimdAabb> for IntersectionCompositeShapeShapeVisitor<'_, D, G1>
84where
85    D: ?Sized + QueryDispatcher,
86    G1: ?Sized + TypedSimdCompositeShape,
87{
88    fn visit(
89        &mut self,
90        bv: &SimdAabb,
91        data: Option<[Option<&G1::PartId>; SIMD_WIDTH]>,
92    ) -> SimdVisitStatus {
93        let mask = self.ls_aabb2.intersects(bv);
94
95        if let Some(data) = data {
96            let bitmask = mask.bitmask();
97            let mut found_intersection = false;
98
99            for (ii, data) in data.into_iter().enumerate() {
100                if (bitmask & (1 << ii)) != 0 {
101                    let Some(data) = data else { continue };
102                    let part_id = *data;
103                    self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| {
104                        found_intersection = self.dispatcher.intersection_test(
105                            &part_pos1.inv_mul(self.pos12),
106                            g1,
107                            self.g2,
108                        ) == Ok(true);
109                    });
110
111                    if found_intersection {
112                        self.found_intersection = Some(part_id);
113                        return SimdVisitStatus::ExitEarly;
114                    }
115                }
116            }
117        }
118
119        SimdVisitStatus::MaybeContinue(mask)
120    }
121}
parry3d_f64/query/intersection_test/intersection_test_composite_shape_shape.rs

parry3d_f64/query/intersection_test/
intersection_test_composite_shape_shape.rs
