ncollide3d/shape/
compound.rs

1//!
2//! Shape composed from the union of primitives.
3//!
4
5use crate::bounding_volume::{BoundingVolume, AABB};
6use crate::math::Isometry;
7use crate::partitioning::{BVHImpl, BVT};
8use crate::query::{Contact, ContactKinematic, ContactPrediction, ContactPreprocessor};
9use crate::shape::{CompositeShape, FeatureId, Shape, ShapeHandle};
10use na::{self, RealField};
11use std::mem;
12
13/// A compound shape with an aabb bounding volume.
14///
15/// A compound shape is a shape composed of the union of several simpler shape. This is
16/// the main way of creating a concave shape from convex parts. Each parts can have its own
17/// delta transformation to shift or rotate it with regard to the other shapes.
18#[derive(Clone)]
19pub struct Compound<N: RealField + Copy> {
20    shapes: Vec<(Isometry<N>, ShapeHandle<N>)>,
21    bvt: BVT<usize, AABB<N>>,
22    bvs: Vec<AABB<N>>,
23    nbits: usize,
24}
25
26impl<N: RealField + Copy> Compound<N> {
27    /// Builds a new compound shape.
28    pub fn new(shapes: Vec<(Isometry<N>, ShapeHandle<N>)>) -> Compound<N> {
29        let mut bvs = Vec::new();
30        let mut leaves = Vec::new();
31
32        for (i, &(ref delta, ref shape)) in shapes.iter().enumerate() {
33            // loosen for better persistancy
34            let bv = shape.as_ref().aabb(delta).loosened(na::convert(0.04f64));
35
36            bvs.push(bv.clone());
37            leaves.push((i, bv));
38
39            if let Some(_comp) = shape.as_composite_shape() {
40                panic!("Nested composite shapes are not allowed.");
41            }
42        }
43
44        let nbits = mem::size_of::<usize>() * 8 - leaves.len().leading_zeros() as usize;
45        let bvt = BVT::new_balanced(leaves);
46
47        Compound {
48            shapes: shapes,
49            bvt: bvt,
50            bvs: bvs,
51            nbits,
52        }
53    }
54}
55
56impl<N: RealField + Copy> Compound<N> {
57    /// The shapes of this compound shape.
58    #[inline]
59    pub fn shapes(&self) -> &[(Isometry<N>, ShapeHandle<N>)] {
60        &self.shapes[..]
61    }
62
63    /// The optimization structure used by this compound shape.
64    #[inline]
65    pub fn bvt(&self) -> &BVT<usize, AABB<N>> {
66        &self.bvt
67    }
68
69    /// The AABB of this compound in its local-space.
70    #[inline]
71    pub fn aabb(&self) -> &AABB<N> {
72        self.bvt()
73            .root_bounding_volume()
74            .expect("An empty Compound has no AABB.")
75    }
76
77    /// The shapes bounding volumes.
78    #[inline]
79    pub fn bounding_volumes(&self) -> &[AABB<N>] {
80        &self.bvs[..]
81    }
82
83    /// The AABB of the i-th shape compositing this compound.
84    #[inline]
85    pub fn aabb_at(&self, i: usize) -> &AABB<N> {
86        &self.bvs[i]
87    }
88
89    /// Transforms a FeatureId of this compound into a pair containing the index of the subshape
90    /// containing this feature, and the corresponding FeatureId on this subshape.
91    pub fn subshape_feature_id(&self, fid: FeatureId) -> (usize, FeatureId) {
92        match fid {
93            FeatureId::Face(i) => (
94                (i & !(usize::max_value() << self.nbits)),
95                FeatureId::Face(i >> self.nbits),
96            ),
97            #[cfg(feature = "dim3")]
98            FeatureId::Edge(i) => (
99                (i & !(usize::max_value() << self.nbits)),
100                FeatureId::Edge(i >> self.nbits),
101            ),
102            FeatureId::Vertex(i) => (
103                (i & !(usize::max_value() << self.nbits)),
104                FeatureId::Vertex(i >> self.nbits),
105            ),
106            FeatureId::Unknown => (0, FeatureId::Unknown),
107        }
108    }
109}
110
111impl<N: RealField + Copy> CompositeShape<N> for Compound<N> {
112    #[inline]
113    fn nparts(&self) -> usize {
114        self.shapes.len()
115    }
116
117    #[inline(always)]
118    fn map_part_at(
119        &self,
120        i: usize,
121        m: &Isometry<N>,
122        f: &mut dyn FnMut(&Isometry<N>, &dyn Shape<N>),
123    ) {
124        let elt = &self.shapes()[i];
125        let pos = m * elt.0;
126
127        f(&pos, elt.1.as_ref())
128    }
129
130    fn map_part_and_preprocessor_at(
131        &self,
132        i: usize,
133        m: &Isometry<N>,
134        _prediction: &ContactPrediction<N>,
135        f: &mut dyn FnMut(&Isometry<N>, &dyn Shape<N>, &dyn ContactPreprocessor<N>),
136    ) {
137        let elt = &self.shapes()[i];
138        let pos = m * elt.0;
139        let proc = CompoundContactProcessor::new(&elt.0, i, self.nbits);
140
141        f(&pos, elt.1.as_ref(), &proc)
142    }
143
144    #[inline]
145    fn aabb_at(&self, i: usize) -> AABB<N> {
146        self.bounding_volumes()[i].clone()
147    }
148
149    #[inline]
150    fn bvh(&self) -> BVHImpl<N, usize, AABB<N>> {
151        BVHImpl::BVT(&self.bvt)
152    }
153}
154
155struct CompoundContactProcessor<'a, N: RealField + Copy> {
156    part_pos: &'a Isometry<N>,
157    part_id: usize,
158    nbits: usize,
159}
160
161impl<'a, N: RealField + Copy> CompoundContactProcessor<'a, N> {
162    pub fn new(part_pos: &'a Isometry<N>, part_id: usize, nbits: usize) -> Self {
163        CompoundContactProcessor {
164            part_pos,
165            part_id,
166            nbits,
167        }
168    }
169}
170
171impl<'a, N: RealField + Copy> ContactPreprocessor<N> for CompoundContactProcessor<'a, N> {
172    fn process_contact(
173        &self,
174        _c: &mut Contact<N>,
175        kinematic: &mut ContactKinematic<N>,
176        is_first: bool,
177    ) -> bool {
178        // Fix the feature ID.
179        let feature = if is_first {
180            kinematic.feature1()
181        } else {
182            kinematic.feature2()
183        };
184
185        let actual_feature = match feature {
186            FeatureId::Vertex(i) => FeatureId::Vertex((i << self.nbits) | self.part_id),
187            #[cfg(feature = "dim3")]
188            FeatureId::Edge(i) => FeatureId::Edge((i << self.nbits) | self.part_id),
189            FeatureId::Face(i) => FeatureId::Face((i << self.nbits) | self.part_id),
190            FeatureId::Unknown => return false,
191        };
192
193        if is_first {
194            kinematic.set_feature1(actual_feature);
195            // The contact kinematics must be expressed on the local frame of
196            // the compound instead of the sub-shape.
197            kinematic.transform1(self.part_pos);
198        } else {
199            kinematic.set_feature2(actual_feature);
200            // The contact kinematics must be expressed on the local frame of
201            // the compound instead of the sub-shape.
202            kinematic.transform2(self.part_pos);
203        }
204
205        true
206    }
207}