f3l_features/
bounding.rs

1use f3l_core::{
2    compute_covariance_matrix, get_minmax,
3    glam::{Mat3A, Vec2, Vec3A},
4    jacobi_eigen_square_n,
5    serde::{self, Deserialize, Serialize},
6    BasicFloat, EigenSet, F3lCast,
7};
8
9/// Compute AABB. A wrapper of [`get_minmax`].
10#[inline]
11pub fn aabb<P, T: BasicFloat, const D: usize>(cloud: &[P]) -> (P, P)
12where
13    P: Into<[T; D]> + Clone + Copy,
14    [T; D]: Into<P>,
15{
16    get_minmax(cloud)
17}
18
19/// Compute Oriented Bounding Box
20///
21/// 1. Compute covariance of data.
22/// 2. Compute eigenvalues and eigenvectors of covariance.
23/// 3. Make each direction orthogonal by cross product.
24/// 4. Multiply inverse of eigenvector to align data to orthogonal.
25/// 5. Compute AABB of aligned data as `Length`
26///
27/// # Examples
28///
29/// ```
30/// let vertices = load_ply("../../data/table_voxel_down.ply");
31/// let obb = OBB::compute(&vertices);
32/// ```
33///
34#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
35#[serde(crate = "self::serde")]
36pub struct OBB<T: BasicFloat, const D: usize, P>
37where
38    P: Into<[T; D]> + Clone + Copy + Send + Sync + std::ops::Index<usize, Output = T>,
39    [T; D]: Into<P>,
40{
41    /// Center of OBB
42    pub center: P,
43    /// Largest eigenvector of eigenvalue
44    pub primary: P,
45    /// second one
46    pub secondary: P,
47    /// Smallest one
48    pub tertiary: P,
49    /// Length of 3 directions.
50    pub length: P,
51}
52
53impl<T: BasicFloat, const D: usize, P> OBB<T, D, P>
54where
55    P: Into<[T; D]> + Clone + Copy + Send + Sync + std::ops::Index<usize, Output = T>,
56    [T; D]: Into<P>,
57{
58    pub fn compute(cloud: &[P]) -> Self {
59        assert!(D == 2 || D == 3);
60
61        let (cov, _) = compute_covariance_matrix(cloud);
62
63        let eigen = EigenSet(jacobi_eigen_square_n(cov));
64
65        let major: Vec3A = if D == 3 {
66            eigen[0].eigenvector.cast_f32::<3>().into()
67        } else {
68            Vec3A::from(Vec2::from(eigen[0].eigenvector.cast_f32::<2>()).extend(0f32))
69        }
70        .normalize();
71        let second: Vec3A = if D == 3 {
72            eigen[1].eigenvector.cast_f32::<3>().into()
73        } else {
74            Vec3A::from(Vec2::from(eigen[1].eigenvector.cast_f32::<2>()).extend(0f32))
75        }
76        .normalize();
77
78        let third = if D == 3 {
79            major.cross(second)
80        } else {
81            Vec3A::Z
82        }
83        .normalize();
84        let second = third.cross(major);
85        let major = second.cross(third);
86
87        let mat = Mat3A::from_cols(major, second, third);//.inverse();
88
89        let mut mean = Vec3A::ZERO;
90        let mut min = Vec3A::MAX;
91        let mut max = Vec3A::MIN;
92
93        cloud.iter().for_each(|p| {
94            for i in 0..D {
95                mean[i] += p[i].to_f32().unwrap();
96            }
97        });
98        mean /= cloud.len() as f32;
99
100        cloud.iter().for_each(|p| {
101            let mut slice = [0f32, 0., 0.];
102            for i in 0..D {
103                slice[i] = p[i].to_f32().unwrap();
104            }
105            let p: Vec3A = slice.into();
106            let x = (p - mean).dot(major);
107            let y = (p - mean).dot(second);
108            let z = (p - mean).dot(third);
109
110            min.x = min.x.min(x);
111            min.y = min.y.min(y);
112            min.z = min.z.min(z);
113            max.x = max.x.max(x);
114            max.y = max.y.max(y);
115            max.z = max.z.max(z);
116        });
117        let shift = (max + min) * 0.5f32;
118
119        let aligned_center = mean + mat.mul_vec3a(shift);
120        let mut center = [T::zero(); D];
121        let mut primary = [T::zero(); D];
122        let mut secondary = [T::zero(); D];
123        let mut tertiary = [T::zero(); D];
124        let mut length = [T::zero(); D];
125        (0..D).for_each(|i| {
126            center[i] = T::from(aligned_center[i]).unwrap();
127            primary[i] = T::from(major[i]).unwrap();
128            secondary[i] = T::from(second[i]).unwrap();
129            tertiary[i] = T::from(third[i]).unwrap();
130            length[i] = T::from((max[i] - min[i]) * 0.5f32).unwrap().abs();
131        });
132
133        Self {
134            center: center.into(),
135            primary: primary.into(),
136            secondary: secondary.into(),
137            tertiary: tertiary.into(),
138            length: length.into(),
139        }
140    }
141}
142
143#[test]
144fn obb_2d() {
145    use f3l_core::round_slice_n;
146    let cloud = vec![[1f32, 0.], [0., 1.], [3., 2.], [2., 3.]];
147    let result = OBB::compute(&cloud);
148
149    assert_eq!([1.5f32, 1.5], round_slice_n(result.center, 4));
150    assert_eq!([1.4142, 0.7071], round_slice_n(result.length, 4));
151}
f3l_features/bounding.rs

f3l_features/
bounding.rs
