parry3d/bounding_volume/
aabb_utils.rs

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

parry3d/bounding_volume/
aabb_utils.rs
