parry3d_f64/bounding_volume/
aabb_utils.rs

1use core::iter::IntoIterator;
2
3use crate::bounding_volume::Aabb;
4use crate::math::{Isometry, Point, Real, Vector, DIM};
5use crate::shape::SupportMap;
6use na;
7
8/// Computes the [`Aabb`] of an [support mapped shape](SupportMap).
9#[cfg(feature = "dim3")]
10pub fn support_map_aabb<G>(m: &Isometry<Real>, i: &G) -> Aabb
11where
12    G: SupportMap,
13{
14    let mut min = na::zero::<Vector<Real>>();
15    let mut max = na::zero::<Vector<Real>>();
16    let mut basis = na::zero::<Vector<Real>>();
17
18    for d in 0..DIM {
19        // TODO: this could be further improved iterating on `m`'s columns, and passing
20        // Id as the transformation matrix.
21        basis[d] = 1.0;
22        max[d] = i.support_point(m, &basis)[d];
23
24        basis[d] = -1.0;
25        min[d] = i.support_point(m, &basis)[d];
26
27        basis[d] = 0.0;
28    }
29
30    Aabb::new(Point::from(min), Point::from(max))
31}
32
33/// Computes the [`Aabb`] of an [support mapped shape](SupportMap).
34pub fn local_support_map_aabb<G>(i: &G) -> Aabb
35where
36    G: SupportMap,
37{
38    let mut min = na::zero::<Vector<Real>>();
39    let mut max = na::zero::<Vector<Real>>();
40    let mut basis = na::zero::<Vector<Real>>();
41
42    for d in 0..DIM {
43        // TODO: this could be further improved iterating on `m`'s columns, and passing
44        // Id as the transformation matrix.
45        basis[d] = 1.0;
46        max[d] = i.local_support_point(&basis)[d];
47
48        basis[d] = -1.0;
49        min[d] = i.local_support_point(&basis)[d];
50
51        basis[d] = 0.0;
52    }
53
54    Aabb::new(Point::from(min), Point::from(max))
55}
56
57/// Computes the [`Aabb`] of a set of points transformed by `m`.
58pub fn point_cloud_aabb<'a, I>(m: &Isometry<Real>, pts: I) -> Aabb
59where
60    I: IntoIterator<Item = &'a Point<Real>>,
61{
62    let mut it = pts.into_iter();
63
64    let p0 = it.next().expect(
65        "Point cloud Aabb construction: the input iterator should yield at least one point.",
66    );
67    let wp0 = m.transform_point(p0);
68    let mut min: Point<Real> = wp0;
69    let mut max: Point<Real> = wp0;
70
71    for pt in it {
72        let wpt = m * pt;
73        min = min.inf(&wpt);
74        max = max.sup(&wpt);
75    }
76
77    Aabb::new(min, max)
78}
79
80/// Computes the [`Aabb`] of a set of points.
81pub fn local_point_cloud_aabb<'a, I>(pts: I) -> Aabb
82where
83    I: IntoIterator<Item = &'a Point<Real>>,
84{
85    let mut it = pts.into_iter();
86
87    let p0 = it.next().expect(
88        "Point cloud Aabb construction: the input iterator should yield at least one point.",
89    );
90    let mut min: Point<Real> = *p0;
91    let mut max: Point<Real> = *p0;
92
93    for pt in it {
94        min = min.inf(pt);
95        max = max.sup(pt);
96    }
97
98    Aabb::new(min, max)
99}
parry3d_f64/bounding_volume/aabb_utils.rs

parry3d_f64/bounding_volume/
aabb_utils.rs
