parry3d/utils/
obb.rs

1use crate::math::{MatExt, Pose, Real, Rotation, Vector, DIM};
2use crate::shape::Cuboid;
3
4/// Computes an oriented bounding box for the given set of points.
5///
6/// The returned OBB is not guaranteed to be the smallest enclosing OBB.
7/// Though it should be a pretty good on for most purposes.
8pub fn obb(pts: &[Vector]) -> (Pose, Cuboid) {
9    let cov = crate::utils::cov(pts);
10    let mut eigv = cov.symmetric_eigen().eigenvectors;
11
12    if eigv.determinant() < 0.0 {
13        eigv = -eigv;
14    }
15
16    let mut mins = Vector::splat(Real::MAX);
17    let mut maxs = Vector::splat(-Real::MAX);
18
19    for pt in pts {
20        for i in 0..DIM {
21            let dot = eigv.col(i).dot(*pt);
22            mins[i] = mins[i].min(dot);
23            maxs[i] = maxs[i].max(dot);
24        }
25    }
26
27    #[cfg(feature = "dim2")]
28    let rot = {
29        // Extract rotation from 2x2 matrix
30        // Matrix rows are [cos θ, -sin θ; sin θ, cos θ]
31        Rotation::from_matrix_unchecked(eigv)
32    };
33    #[cfg(feature = "dim3")]
34    let rot = Rotation::from_mat3(&eigv);
35
36    // Create the isometry with rotation and the rotated center translation
37    let center = (maxs + mins) / 2.0;
38    let rotated_center = rot * center;
39    (
40        Pose::from_parts(rotated_center, rot),
41        Cuboid::new((maxs - mins) / 2.0),
42    )
43}