ncollide3d/shape/
compound.rs
1use 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#[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 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 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 #[inline]
59 pub fn shapes(&self) -> &[(Isometry<N>, ShapeHandle<N>)] {
60 &self.shapes[..]
61 }
62
63 #[inline]
65 pub fn bvt(&self) -> &BVT<usize, AABB<N>> {
66 &self.bvt
67 }
68
69 #[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 #[inline]
79 pub fn bounding_volumes(&self) -> &[AABB<N>] {
80 &self.bvs[..]
81 }
82
83 #[inline]
85 pub fn aabb_at(&self, i: usize) -> &AABB<N> {
86 &self.bvs[i]
87 }
88
89 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 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 kinematic.transform1(self.part_pos);
198 } else {
199 kinematic.set_feature2(actual_feature);
200 kinematic.transform2(self.part_pos);
203 }
204
205 true
206 }
207}