parry3d/bounding_volume/
aabb_utils.rs

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

parry3d/bounding_volume/
aabb_utils.rs
