ncollide3d/shape/
cuboid.rs

1//! Support mapping based Cuboid shape.
2
3use crate::math::{Isometry, Point, Vector, DIM};
4use crate::shape::{ConvexPolygonalFeature, ConvexPolyhedron, FeatureId, SupportMap};
5use na::{self, RealField, Unit};
6use std::f64;
7
8/// Shape of a box.
9#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
10#[derive(PartialEq, Debug, Copy, Clone)]
11pub struct Cuboid<N: RealField + Copy> {
12    /// The half-extents of the cuboid.
13    pub half_extents: Vector<N>,
14}
15
16// NOTE: format of the cuboid feature id:
17//
18// FeatureId::Vertex(id): the i-th bit of `id` is set to 1 iff. the i-th component of the vertex is negative.
19// FeatureId::Edge(id): the part `id & 0b11` contains a number in [0,1] (or [0,2] in 3D) to indicate the axis (x, y, z).
20//                      the part `id >> 2` follow the same rule as the vertex id.
21// FeatureId::Face(id): if `id` lies in [0,1] (or [0 2] in 3D) indicates the axis (x, y, z) corresponding to the face normal.
22//                      If `id` is greater than 1 (or 2 in 3D), then the negative axis (-x, -y, -z) is given by `id - 2` (or `id - 3` in 3D).
23impl<N: RealField + Copy> Cuboid<N> {
24    /// Creates a new box from its half-extents. Half-extents are the box half-width along each
25    /// axis. Each half-extent must be positive.
26    #[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    /// The half-extents of this box. Half-extents are the box half-width along each axis.
34    #[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    /// Checks that the given direction in world-space is on the tangent cone of the given `feature`.
41    #[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    /// Checks that the given direction in world-space is on the tangent cone of the given `feature`.
70    #[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); // The masks are for ensuring each edge has a unique ID.
238            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        // FIXME: we should use nalgebra's iamax method.
292        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            // We are not on a face, return the support vertex.
345            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            // Check faces.
358            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            // Check edges.
378            for i in 0..3 {
379                let sign = local_dir[i].signum();
380
381                // sign * local_dir[i] <= cos(pi / 2 - angle)
382                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            // We are not on a face or edge, return the support vertex.
399            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            // We are not on a face, return the support vertex.
430            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            // Check faces.
439            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            // Check edges.
455            for i in 0..3 {
456                let sign = local_dir[i].signum();
457
458                // sign * local_dir[i] <= cos(pi / 2 - angle)
459                if sign * local_dir[i] <= sang {
460                    let mask_i = !(1 << i); // To ensure each edge has a unique id.
461                    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}