parry3d/bounding_volume/
aabb_utils.rs1use 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#[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 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
33pub 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 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
57pub fn point_cloud_aabb_ref<'a, I>(m: &Isometry<Real>, pts: I) -> Aabb
59where
60 I: IntoIterator<Item = &'a Point<Real>>,
61{
62 point_cloud_aabb(m, pts.into_iter().copied())
63}
64
65pub fn point_cloud_aabb<I>(m: &Isometry<Real>, pts: I) -> Aabb
67where
68 I: IntoIterator<Item = Point<Real>>,
69{
70 let mut it = pts.into_iter();
71
72 let p0 = it.next().expect(
73 "Point cloud Aabb construction: the input iterator should yield at least one point.",
74 );
75 let wp0 = m.transform_point(&p0);
76 let mut min: Point<Real> = wp0;
77 let mut max: Point<Real> = wp0;
78
79 for pt in it {
80 let wpt = m * pt;
81 min = min.inf(&wpt);
82 max = max.sup(&wpt);
83 }
84
85 Aabb::new(min, max)
86}
87
88pub fn local_point_cloud_aabb_ref<'a, I>(pts: I) -> Aabb
90where
91 I: IntoIterator<Item = &'a Point<Real>>,
92{
93 local_point_cloud_aabb(pts.into_iter().copied())
94}
95
96pub fn local_point_cloud_aabb<I>(pts: I) -> Aabb
98where
99 I: IntoIterator<Item = Point<Real>>,
100{
101 let mut it = pts.into_iter();
102
103 let p0 = it.next().expect(
104 "Point cloud Aabb construction: the input iterator should yield at least one point.",
105 );
106 let mut min: Point<Real> = p0;
107 let mut max: Point<Real> = p0;
108
109 for pt in it {
110 min = min.inf(&pt);
111 max = max.sup(&pt);
112 }
113
114 Aabb::new(min, max)
115}