1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147

use f3l_core::{
    compute_covariance_matrix, get_minmax,
    glam::{Mat3A, Vec2, Vec3A},
    jacobi_eigen_square_n,
    serde::{self, Deserialize, Serialize},
    BasicFloat, EigenSet, F3lCast,
};

/// Compute AABB. A wrapper of [`get_minmax`].
#[inline]
pub fn aabb<P, T: BasicFloat, const D: usize>(cloud: &[P]) -> (P, P)
where
    P: Into<[T; D]> + Clone + Copy,
    [T; D]: Into<P>,
{
    get_minmax(cloud)
}

/// Compute Oriented Bounding Box
///
/// 1. Compute covariance of data.
/// 2. Compute eigenvalues and eigenvectors of covariance.
/// 3. Make each direction orthogonal by cross product.
/// 4. Multiply inverse of eigenvector to align data to orthogonal.
/// 5. Compute AABB of aligned data as `Length`
///
/// # Examples
///
/// ```
/// let vertices = load_ply("../../data/table_voxel_down.ply");
/// let obb = OBB::compute(&vertices);
/// ```
///
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
#[serde(crate = "self::serde")]
pub struct OBB<T: BasicFloat, const D: usize, P>
where
    P: Into<[T; D]> + Clone + Copy + Send + Sync + std::ops::Index<usize, Output = T>,
    [T; D]: Into<P>,
{
    /// Center of OBB
    pub center: P,
    /// Largest eigenvector of eigenvalue
    pub primary: P,
    /// second one
    pub secondary: P,
    /// Smallest one
    pub tertiary: P,
    /// Length of 3 directions.
    pub length: P,
}

impl<T: BasicFloat, const D: usize, P> OBB<T, D, P>
where
    P: Into<[T; D]> + Clone + Copy + Send + Sync + std::ops::Index<usize, Output = T>,
    [T; D]: Into<P>,
{
    pub fn compute(cloud: &[P]) -> Self {
        assert!(D <= 3);

        let (cov, _) = compute_covariance_matrix(cloud);

        let eigen = EigenSet(jacobi_eigen_square_n(cov));

        let major: Vec3A = if D == 3 {
            eigen[0].eigenvector.cast_f32::<3>().into()
        } else {
            Vec3A::from(Vec2::from(eigen[0].eigenvector.cast_f32::<2>()).extend(0f32))
        }
        .normalize();
        let second: Vec3A = if D == 3 {
            eigen[1].eigenvector.cast_f32::<3>().into()
        } else {
            Vec3A::from(Vec2::from(eigen[1].eigenvector.cast_f32::<2>()).extend(0f32))
        }
        .normalize();

        let mut third = if D == 3 {
            major.cross(second)
        } else {
            Vec3A::Z
        }
        .normalize();
        let mut second = third.cross(major);
        let mut major = second.cross(third);

        (0..D - 1).for_each(|i| {
            major[i] *= -1f32;
            second[i] *= -1f32;
            third[i] *= -1f32;
        });

        let mat = Mat3A::from_cols(major, second, third).inverse();

        let align = cloud
            .iter()
            .map(|&p| {
                if D == 3 {
                    mat.mul_vec3a(Vec3A::new(
                        p[0].to_f32().unwrap(),
                        p[1].to_f32().unwrap(),
                        p[2].to_f32().unwrap(),
                    ))
                } else {
                    mat.mul_vec3a(Vec3A::new(
                        p[0].to_f32().unwrap(),
                        p[1].to_f32().unwrap(),
                        0f32,
                    ))
                }
            })
            .collect::<Vec<Vec3A>>();
        let (min, max) = aabb::<Vec3A, f32, 3>(&align);
        let aligned_center = mat.mul_vec3a((max + min) * 0.5f32);

        let mut center = [T::zero(); D];
        let mut primary = [T::zero(); D];
        let mut secondary = [T::zero(); D];
        let mut tertiary = [T::zero(); D];
        let mut length = [T::zero(); D];
        (0..D).for_each(|i| {
            center[i] = T::from(aligned_center[i]).unwrap();
            primary[i] = T::from(major[i]).unwrap();
            secondary[i] = T::from(second[i]).unwrap();
            tertiary[i] = T::from(third[i]).unwrap();
            length[i] = T::from((max[i] - min[i]) / 2f32).unwrap().abs();
        });

        Self {
            center: center.into(),
            primary: primary.into(),
            secondary: secondary.into(),
            tertiary: tertiary.into(),
            length: length.into(),
        }
    }
}

#[test]
fn obb_2d() {
    use f3l_core::round_slice_n;
    let cloud = vec![[1f32, 0.], [0., 1.], [3., 2.], [2., 3.]];
    let result = OBB::compute(&cloud);

    assert_eq!([1.5f32, 1.5], round_slice_n(result.center, 4));
    assert_eq!([0.7071, 1.4142], round_slice_n(result.length, 4));
}