ncollide3d/shape/
cuboid.rs
1use crate::math::{Isometry, Point, Vector, DIM};
4use crate::shape::{ConvexPolygonalFeature, ConvexPolyhedron, FeatureId, SupportMap};
5use na::{self, RealField, Unit};
6use std::f64;
7
8#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
10#[derive(PartialEq, Debug, Copy, Clone)]
11pub struct Cuboid<N: RealField + Copy> {
12 pub half_extents: Vector<N>,
14}
15
16impl<N: RealField + Copy> Cuboid<N> {
24 #[inline]
27 pub fn new(half_extents: Vector<N>) -> Cuboid<N> {
28 Cuboid { half_extents }
29 }
30}
31
32impl<N: RealField + Copy> Cuboid<N> {
33 #[inline]
35 #[deprecated(note = "use the `self.half_extents` public field directly.")]
36 pub fn half_extents(&self) -> &Vector<N> {
37 &self.half_extents
38 }
39
40 #[cfg(feature = "dim2")]
42 pub fn tangent_cone_contains_dir(
43 &self,
44 feature: FeatureId,
45 m: &Isometry<N>,
46 dir: &Unit<Vector<N>>,
47 ) -> bool {
48 let ls_dir = m.inverse_transform_vector(dir);
49
50 match feature {
51 FeatureId::Face(id) => {
52 if id < 2 {
53 ls_dir[id] <= N::zero()
54 } else {
55 ls_dir[id - 2] >= N::zero()
56 }
57 }
58 FeatureId::Vertex(id) => match id {
59 0b00 => ls_dir.x <= N::zero() && ls_dir.y <= N::zero(),
60 0b01 => ls_dir.x >= N::zero() && ls_dir.y <= N::zero(),
61 0b11 => ls_dir.x >= N::zero() && ls_dir.y >= N::zero(),
62 0b10 => ls_dir.x <= N::zero() && ls_dir.y >= N::zero(),
63 _ => unreachable!(),
64 },
65 _ => panic!("Invalid feature ID {:?}.", feature),
66 }
67 }
68
69 #[cfg(feature = "dim3")]
71 pub fn tangent_cone_contains_dir(
72 &self,
73 feature: FeatureId,
74 m: &Isometry<N>,
75 dir: &Unit<Vector<N>>,
76 ) -> bool {
77 let ls_dir = m.inverse_transform_vector(dir);
78
79 match feature {
80 FeatureId::Face(id) => {
81 if id < 3 {
82 ls_dir[id] <= N::zero()
83 } else {
84 ls_dir[id - 3] >= N::zero()
85 }
86 }
87 FeatureId::Edge(id) => {
88 let edge = id & 0b011;
89 let face1 = (edge + 1) % 3;
90 let face2 = (edge + 2) % 3;
91 let signs = id >> 2;
92
93 if signs & (1 << face1) != 0 {
94 if ls_dir[face1] < N::zero() {
95 return false;
96 }
97 } else {
98 if ls_dir[face1] > N::zero() {
99 return false;
100 }
101 }
102
103 if signs & (1 << face2) != 0 {
104 if ls_dir[face2] < N::zero() {
105 return false;
106 }
107 } else {
108 if ls_dir[face2] > N::zero() {
109 return false;
110 }
111 }
112
113 true
114 }
115 FeatureId::Vertex(id) => {
116 for i in 0..3 {
117 if id & (1 << i) != 0 {
118 if ls_dir[i] < N::zero() {
119 return false;
120 }
121 } else {
122 if ls_dir[i] > N::zero() {
123 return false;
124 }
125 }
126 }
127
128 true
129 }
130 _ => false,
131 }
132 }
133}
134
135impl<N: RealField + Copy> SupportMap<N> for Cuboid<N> {
136 #[inline]
137 fn local_support_point(&self, dir: &Vector<N>) -> Point<N> {
138 let mut res = self.half_extents;
139
140 for i in 0usize..DIM {
141 res[i] = res[i].copysign(dir[i]);
142 }
143
144 res.into()
145 }
146}
147
148impl<N: RealField + Copy> ConvexPolyhedron<N> for Cuboid<N> {
149 fn vertex(&self, id: FeatureId) -> Point<N> {
150 let vid = id.unwrap_vertex();
151 let mut res = self.half_extents;
152
153 for i in 0..DIM {
154 if vid & (1 << i) != 0 {
155 res[i] = -res[i]
156 }
157 }
158
159 Point::from(res)
160 }
161
162 #[cfg(feature = "dim3")]
163 fn edge(&self, id: FeatureId) -> (Point<N>, Point<N>, FeatureId, FeatureId) {
164 let eid = id.unwrap_edge();
165 let mut res = self.half_extents;
166
167 let edge_i = eid & 0b11;
168 let vertex_i = eid >> 2;
169
170 for i in 0..DIM {
171 if i != edge_i && (vertex_i & (1 << i) != 0) {
172 res[i] = -res[i]
173 }
174 }
175
176 let p1 = Point::from(res);
177 res[edge_i] = -res[edge_i];
178 let p2 = Point::from(res);
179 let vid1 = FeatureId::Vertex(vertex_i & !(1 << edge_i));
180 let vid2 = FeatureId::Vertex(vertex_i | (1 << edge_i));
181
182 (p1, p2, vid1, vid2)
183 }
184
185 fn face(&self, id: FeatureId, out: &mut ConvexPolygonalFeature<N>) {
186 out.clear();
187
188 let i = id.unwrap_face();
189 let i1;
190 let sign;
191
192 if i < DIM {
193 i1 = i;
194 sign = N::one();
195 } else {
196 i1 = i - DIM;
197 sign = -N::one();
198 }
199
200 #[cfg(feature = "dim2")]
201 {
202 let i2 = (i1 + 1) % 2;
203
204 let mut vertex = self.half_extents;
205 vertex[i1] *= sign;
206 vertex[i2] *= if i1 == 0 { -sign } else { sign };
207
208 let p1 = Point::from(vertex);
209 vertex[i2] = -vertex[i2];
210 let p2 = Point::from(vertex);
211
212 let mut vertex_id1 = if sign < na::zero() { 1 << i1 } else { 0 };
213 let mut vertex_id2 = vertex_id1;
214 if p1[i2] < na::zero() {
215 vertex_id1 |= 1 << i2;
216 } else {
217 vertex_id2 |= 1 << i2;
218 }
219
220 out.push(p1, FeatureId::Vertex(vertex_id1));
221 out.push(p2, FeatureId::Vertex(vertex_id2));
222
223 let mut normal: Vector<N> = na::zero();
224 normal[i1] = sign;
225 out.set_normal(Unit::new_unchecked(normal));
226 out.set_feature_id(FeatureId::Face(i));
227 }
228 #[cfg(feature = "dim3")]
229 {
230 let i2 = (i1 + 1) % 3;
231 let i3 = (i1 + 2) % 3;
232 let (edge_i2, edge_i3) = if sign > na::zero() {
233 (i2, i3)
234 } else {
235 (i3, i2)
236 };
237 let mask_i2 = !(1 << edge_i2); let mask_i3 = !(1 << edge_i3);
239 let mut vertex = self.half_extents;
240 vertex[i1] *= sign;
241
242 let (sbit, msbit) = if sign < na::zero() { (1, 0) } else { (0, 1) };
243 let mut vertex_id = sbit << i1;
244 out.push(Point::from(vertex), FeatureId::Vertex(vertex_id));
245 out.push_edge_feature_id(FeatureId::Edge(edge_i2 | ((vertex_id & mask_i2) << 2)));
246
247 vertex[i2] = -sign * self.half_extents[i2];
248 vertex[i3] = sign * self.half_extents[i3];
249 vertex_id |= msbit << i2 | sbit << i3;
250 out.push(Point::from(vertex), FeatureId::Vertex(vertex_id));
251 out.push_edge_feature_id(FeatureId::Edge(edge_i3 | ((vertex_id & mask_i3) << 2)));
252
253 vertex[i2] = -self.half_extents[i2];
254 vertex[i3] = -self.half_extents[i3];
255 vertex_id |= 1 << i2 | 1 << i3;
256 out.push(Point::from(vertex), FeatureId::Vertex(vertex_id));
257 out.push_edge_feature_id(FeatureId::Edge(edge_i2 | ((vertex_id & mask_i2) << 2)));
258
259 vertex[i2] = sign * self.half_extents[i2];
260 vertex[i3] = -sign * self.half_extents[i3];
261 vertex_id = sbit << i1 | sbit << i2 | msbit << i3;
262 out.push(Point::from(vertex), FeatureId::Vertex(vertex_id));
263 out.push_edge_feature_id(FeatureId::Edge(edge_i3 | ((vertex_id & mask_i3) << 2)));
264
265 let mut normal: Vector<N> = na::zero();
266 normal[i1] = sign;
267 out.set_normal(Unit::new_unchecked(normal));
268
269 if sign > na::zero() {
270 out.set_feature_id(FeatureId::Face(i1));
271 } else {
272 out.set_feature_id(FeatureId::Face(i1 + 3));
273 }
274
275 out.recompute_edge_normals();
276 }
277 }
278
279 fn support_face_toward(
280 &self,
281 m: &Isometry<N>,
282 dir: &Unit<Vector<N>>,
283 out: &mut ConvexPolygonalFeature<N>,
284 ) {
285 out.clear();
286 let local_dir = m.inverse_transform_vector(dir);
287
288 let mut iamax = 0;
289 let mut amax = local_dir[0].abs();
290
291 for i in 1..DIM {
293 let candidate = local_dir[i].abs();
294 if candidate > amax {
295 amax = candidate;
296 iamax = i;
297 }
298 }
299
300 if local_dir[iamax] > na::zero() {
301 self.face(FeatureId::Face(iamax), out);
302 out.transform_by(m);
303 } else {
304 self.face(FeatureId::Face(iamax + DIM), out);
305 out.transform_by(m);
306 }
307 }
308
309 fn support_feature_toward(
310 &self,
311 m: &Isometry<N>,
312 dir: &Unit<Vector<N>>,
313 angle: N,
314 out: &mut ConvexPolygonalFeature<N>,
315 ) {
316 let local_dir = m.inverse_transform_vector(dir);
317 let cang = angle.cos();
318 let mut support_point = self.half_extents;
319
320 out.clear();
321
322 #[cfg(feature = "dim2")]
323 {
324 let mut support_point_id = 0;
325 for i1 in 0..2 {
326 let sign = local_dir[i1].signum();
327 if sign * local_dir[i1] >= cang {
328 if sign > na::zero() {
329 self.face(FeatureId::Face(i1), out);
330 out.transform_by(m);
331 } else {
332 self.face(FeatureId::Face(i1 + 2), out);
333 out.transform_by(m);
334 }
335 return;
336 } else {
337 if sign < na::zero() {
338 support_point_id |= 1 << i1;
339 }
340 support_point[i1] *= sign;
341 }
342 }
343
344 out.push(
346 m * Point::from(support_point),
347 FeatureId::Vertex(support_point_id),
348 );
349 out.set_feature_id(FeatureId::Vertex(support_point_id));
350 }
351
352 #[cfg(feature = "dim3")]
353 {
354 let sang = angle.sin();
355 let mut support_point_id = 0;
356
357 for i1 in 0..3 {
359 let sign = local_dir[i1].signum();
360 if sign * local_dir[i1] >= cang {
361 if sign > na::zero() {
362 self.face(FeatureId::Face(i1), out);
363 out.transform_by(m);
364 } else {
365 self.face(FeatureId::Face(i1 + 3), out);
366 out.transform_by(m);
367 }
368 return;
369 } else {
370 if sign < na::zero() {
371 support_point[i1] *= sign;
372 support_point_id |= 1 << i1;
373 }
374 }
375 }
376
377 for i in 0..3 {
379 let sign = local_dir[i].signum();
380
381 if sign * local_dir[i] <= sang {
383 support_point[i] = -self.half_extents[i];
384 let p1 = Point::from(support_point);
385 support_point[i] = self.half_extents[i];
386 let p2 = Point::from(support_point);
387 let p2_id = support_point_id & !(1 << i);
388 out.push(m * p1, FeatureId::Vertex(support_point_id | (1 << i)));
389 out.push(m * p2, FeatureId::Vertex(p2_id));
390
391 let edge_id = FeatureId::Edge(i | (p2_id << 2));
392 out.push_edge_feature_id(edge_id);
393 out.set_feature_id(edge_id);
394 return;
395 }
396 }
397
398 out.push(
400 m * Point::from(support_point),
401 FeatureId::Vertex(support_point_id),
402 );
403 out.set_feature_id(FeatureId::Vertex(support_point_id));
404 }
405 }
406
407 fn support_feature_id_toward(&self, local_dir: &Unit<Vector<N>>) -> FeatureId {
408 let one_degree: N = na::convert(f64::consts::PI / 180.0);
409 let cang = one_degree.cos();
410
411 #[cfg(feature = "dim2")]
412 {
413 let mut support_point_id = 0;
414 for i1 in 0..2 {
415 let sign = local_dir[i1].signum();
416 if sign * local_dir[i1] >= cang {
417 if sign > na::zero() {
418 return FeatureId::Face(i1);
419 } else {
420 return FeatureId::Face(i1 + 2);
421 }
422 } else {
423 if sign < na::zero() {
424 support_point_id |= 1 << i1;
425 }
426 }
427 }
428
429 FeatureId::Vertex(support_point_id)
431 }
432
433 #[cfg(feature = "dim3")]
434 {
435 let sang = one_degree.sin();
436 let mut support_point_id = 0;
437
438 for i1 in 0..3 {
440 let sign = local_dir[i1].signum();
441 if sign * local_dir[i1] >= cang {
442 if sign > na::zero() {
443 return FeatureId::Face(i1);
444 } else {
445 return FeatureId::Face(i1 + 3);
446 }
447 } else {
448 if sign < na::zero() {
449 support_point_id |= 1 << i1;
450 }
451 }
452 }
453
454 for i in 0..3 {
456 let sign = local_dir[i].signum();
457
458 if sign * local_dir[i] <= sang {
460 let mask_i = !(1 << i); return FeatureId::Edge(i | ((support_point_id & mask_i) << 2));
462 }
463 }
464
465 FeatureId::Vertex(support_point_id)
466 }
467 }
468
469 #[cfg(feature = "dim2")]
470 fn feature_normal(&self, feature: FeatureId) -> Unit<Vector<N>> {
471 match feature {
472 FeatureId::Face(id) => {
473 let mut dir: Vector<N> = na::zero();
474
475 if id < 2 {
476 dir[id] = N::one();
477 } else {
478 dir[id - 2] = -N::one();
479 }
480 Unit::new_unchecked(dir)
481 }
482 FeatureId::Vertex(id) => {
483 let mut dir: Vector<N> = na::zero();
484
485 match id {
486 0b00 => {
487 dir[0] = N::one();
488 dir[1] = N::one();
489 }
490 0b01 => {
491 dir[1] = N::one();
492 dir[0] = -N::one();
493 }
494 0b11 => {
495 dir[0] = -N::one();
496 dir[1] = -N::one();
497 }
498 0b10 => {
499 dir[1] = -N::one();
500 dir[0] = N::one();
501 }
502 _ => panic!("Invalid feature ID: {:?}", feature),
503 }
504
505 Unit::new_normalize(dir)
506 }
507 _ => panic!("Invalid feature ID {:?}.", feature),
508 }
509 }
510
511 #[cfg(feature = "dim3")]
512 fn feature_normal(&self, feature: FeatureId) -> Unit<Vector<N>> {
513 match feature {
514 FeatureId::Face(id) => {
515 let mut dir: Vector<N> = na::zero();
516
517 if id < 3 {
518 dir[id] = N::one();
519 } else {
520 dir[id - 3] = -N::one();
521 }
522 Unit::new_unchecked(dir)
523 }
524 FeatureId::Edge(id) => {
525 let edge = id & 0b011;
526 let face1 = (edge + 1) % 3;
527 let face2 = (edge + 2) % 3;
528 let signs = id >> 2;
529
530 let mut dir: Vector<N> = na::zero();
531 let _1: N = na::one();
532
533 if signs & (1 << face1) != 0 {
534 dir[face1] = -_1
535 } else {
536 dir[face1] = _1
537 }
538
539 if signs & (1 << face2) != 0 {
540 dir[face2] = -_1
541 } else {
542 dir[face2] = _1;
543 }
544
545 Unit::new_normalize(dir)
546 }
547 FeatureId::Vertex(id) => {
548 let mut dir: Vector<N> = na::zero();
549 for i in 0..3 {
550 let _1: N = na::one();
551
552 if id & (1 << i) != 0 {
553 dir[i] = -_1;
554 } else {
555 dir[i] = _1
556 }
557 }
558
559 Unit::new_normalize(dir)
560 }
561 _ => panic!("Invalid feature ID: {:?}", feature),
562 }
563 }
564}