ncollide3d/query/proximity/
proximity_composite_shape_shape.rs

Help
1use crate::bounding_volume::AABB;
2use crate::math::{Isometry, Point, Vector};
3use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor};
4use crate::query::{self, PointQuery, Proximity};
5use crate::shape::{CompositeShape, Shape};
6use na::{self, RealField};
7
8/// Proximity between a composite shape (`Mesh`, `Compound`) and any other shape.
9pub fn proximity_composite_shape_shape<N: RealField + Copy, G1: ?Sized>(
10    m1: &Isometry<N>,
11    g1: &G1,
12    m2: &Isometry<N>,
13    g2: &dyn Shape<N>,
14    margin: N,
15) -> Proximity
16where
17    G1: CompositeShape<N>,
18{
19    assert!(
20        margin >= na::zero(),
21        "The proximity margin must be positive or null."
22    );
23
24    let mut visitor = CompositeShapeAgainstAnyInterfVisitor::new(m1, g1, m2, g2, margin);
25
26    match g1.bvh().best_first_search(&mut visitor) {
27        None => Proximity::Disjoint,
28        Some(prox) => prox.1,
29    }
30}
31
32/// Proximity between a shape and a composite (`Mesh`, `Compound`) shape.
33pub fn proximity_shape_composite_shape<N: RealField + Copy, G2: ?Sized>(
34    m1: &Isometry<N>,
35    g1: &dyn Shape<N>,
36    m2: &Isometry<N>,
37    g2: &G2,
38    margin: N,
39) -> Proximity
40where
41    G2: CompositeShape<N>,
42{
43    proximity_composite_shape_shape(m2, g2, m1, g1, margin)
44}
45
46struct CompositeShapeAgainstAnyInterfVisitor<'a, N: 'a + RealField + Copy, G1: ?Sized + 'a> {
47    msum_shift: Vector<N>,
48    msum_margin: Vector<N>,
49
50    m1: &'a Isometry<N>,
51    g1: &'a G1,
52    m2: &'a Isometry<N>,
53    g2: &'a dyn Shape<N>,
54    margin: N,
55}
56
57impl<'a, N: RealField + Copy, G1: ?Sized> CompositeShapeAgainstAnyInterfVisitor<'a, N, G1>
58where
59    G1: CompositeShape<N>,
60{
61    pub fn new(
62        m1: &'a Isometry<N>,
63        g1: &'a G1,
64        m2: &'a Isometry<N>,
65        g2: &'a dyn Shape<N>,
66        margin: N,
67    ) -> CompositeShapeAgainstAnyInterfVisitor<'a, N, G1> {
68        let ls_m2 = m1.inverse() * m2.clone();
69        let ls_aabb2 = g2.aabb(&ls_m2);
70
71        CompositeShapeAgainstAnyInterfVisitor {
72            msum_shift: -ls_aabb2.center().coords,
73            msum_margin: ls_aabb2.half_extents(),
74            m1,
75            g1,
76            m2,
77            g2,
78            margin,
79        }
80    }
81}
82
83impl<'a, N: RealField + Copy, G1: ?Sized> BestFirstVisitor<N, usize, AABB<N>>
84    for CompositeShapeAgainstAnyInterfVisitor<'a, N, G1>
85where
86    G1: CompositeShape<N>,
87{
88    type Result = Proximity;
89
90    fn visit(
91        &mut self,
92        best: N,
93        bv: &AABB<N>,
94        data: Option<&usize>,
95    ) -> BestFirstVisitStatus<N, Self::Result> {
96        // Compute the minkowski sum of the two AABBs.
97        let msum = AABB::new(
98            bv.mins + self.msum_shift + (-self.msum_margin),
99            bv.maxs + self.msum_shift + self.msum_margin,
100        );
101
102        // Compute the distance to the origin.
103        let dist = msum.distance_to_point(&Isometry::identity(), &Point::origin(), true);
104        let mut res = BestFirstVisitStatus::Continue {
105            cost: dist,
106            result: None,
107        };
108
109        if let Some(b) = data {
110            if dist < best {
111                self.g1
112                    .map_part_at(*b, self.m1, &mut |m1, g1| match query::proximity(
113                        m1,
114                        g1,
115                        self.m2,
116                        self.g2,
117                        self.margin,
118                    ) {
119                        Proximity::WithinMargin => {
120                            res = BestFirstVisitStatus::Continue {
121                                cost: self.margin,
122                                result: Some(Proximity::WithinMargin),
123                            }
124                        }
125                        Proximity::Intersecting => {
126                            res = BestFirstVisitStatus::ExitEarly(Some(Proximity::Intersecting))
127                        }
128                        Proximity::Disjoint => {}
129                    });
130            }
131        }
132
133        res
134    }
135}
ncollide3d/query/proximity/proximity_composite_shape_shape.rs

ncollide3d/query/proximity/
proximity_composite_shape_shape.rs
