phys-collision 2.0.1-beta.0

// Copyright (C) 2020-2025 phys-collision authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use glam_det::nums::{f32x4, PartialOrdEx};
use glam_det::{Isometry3x4, Point3x4};
use glamdet_na_conv::ConvTo;

use crate::Aabb3;

///efficiently store and manipulate four AABBs with SIMD
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct Aabb3Wide {
    min: Point3x4, //  The minimum coordinates of the AABB in each lane
    max: Point3x4, // The maximum coordinates of the AABB in each lane.
}

impl Aabb3Wide {
    pub const ANTI_INFINITE: Self = Self {
        min: Point3x4::splat(f32x4::const_splat(f32::MAX)),
        max: Point3x4::splat(f32x4::const_splat(f32::MIN)),
    };
    pub const ZERO: Self = Self {
        min: Point3x4::ZERO,
        max: Point3x4::ZERO,
    };

    /// Select the smaller group of input data as the min and the larger group as the max
    /// to prevent the situation where min is greater than max.
    #[inline]
    #[must_use]
    pub fn new(p1: Point3x4, p2: Point3x4) -> Self {
        Self {
            min: Point3x4::new(
                f32x4::min(p1.x, p2.x),
                f32x4::min(p1.y, p2.y),
                f32x4::min(p1.z, p2.z),
            ),
            max: Point3x4::new(
                f32x4::max(p1.x, p2.x),
                f32x4::max(p1.y, p2.y),
                f32x4::max(p1.z, p2.z),
            ),
        }
    }

    /// Create AABB bound with min and max point with out check
    #[inline]
    #[must_use]
    pub fn new_unchecked(min: Point3x4, max: Point3x4) -> Self {
        Self { min, max }
    }

    ///first 3 element is min, later 3 elements is max
    #[inline]
    #[must_use]
    pub fn from_array_unchecked(values: [f32x4; 6]) -> Self {
        Self {
            min: Point3x4::new(values[0], values[1], values[2]),
            max: Point3x4::new(values[3], values[4], values[5]),
        }
    }

    ///assemble 4 AABBs into a `Aabb3Wide`
    #[inline]
    #[must_use]
    pub fn compose_soa(aabb: &[Aabb3; 4]) -> Self {
        Self {
            min: Point3x4::compose_soa(&[
                aabb[0].min().conv_to(),
                aabb[1].min().conv_to(),
                aabb[2].min().conv_to(),
                aabb[3].min().conv_to(),
            ]),
            max: Point3x4::compose_soa(&[
                aabb[0].max().conv_to(),
                aabb[1].max().conv_to(),
                aabb[2].max().conv_to(),
                aabb[3].max().conv_to(),
            ]),
        }
    }

    /// Returns the minimum point of the AABB.
    #[inline]
    #[must_use]
    pub fn min(&self) -> Point3x4 {
        self.min
    }

    /// Returns the maximum point of the AABB.
    #[inline]
    #[must_use]
    pub fn max(&self) -> Point3x4 {
        self.max
    }

    /// Grow the aabb to include the given point.
    #[inline]
    pub fn grow(&mut self, p: Point3x4) {
        self.min = self.min.min(p);
        self.max = self.max.max(p);
    }

    ///defines the range that the  AABB should encompass
    #[inline]
    #[must_use]
    pub fn corners(&self) -> [Point3x4; 8] {
        let min = self.min;
        let max = self.max;
        [
            min,
            Point3x4::new(min.x, min.y, max.z),
            Point3x4::new(min.x, max.y, min.z),
            Point3x4::new(min.x, max.y, max.z),
            Point3x4::new(max.x, min.y, min.z),
            Point3x4::new(max.x, min.y, max.z),
            Point3x4::new(max.x, max.y, min.z),
            max,
        ]
    }

    /// transform AABB, and contains the corners
    #[inline]
    #[must_use]
    pub fn transform(&self, transform: Isometry3x4) -> Self {
        let corners = self.corners();
        let c0 = transform * corners[0];
        let c1 = transform * corners[1];
        let mut aabb = Aabb3Wide::new(c0, c1);
        for corner in corners.iter().skip(2) {
            aabb.grow(transform * *corner);
        }
        aabb
    }

    /// extract the AABB in the given lane
    #[inline]
    #[must_use]
    pub fn extract(&self, lane: usize) -> Aabb3 {
        Aabb3::new_unchecked(
            self.min.extract_lane(lane).to_array(),
            self.max.extract_lane(lane).to_array(),
        )
    }

    /// decompose the Aabb Wide to 4 Aabb3
    #[inline]
    #[must_use]
    pub fn decompose(&self) -> [Aabb3; 4] {
        let minx: [f32; 4] = self.min.x.into();
        let miny: [f32; 4] = self.min.y.into();
        let minz: [f32; 4] = self.min.z.into();

        let maxx: [f32; 4] = self.max.x.into();
        let maxy: [f32; 4] = self.max.y.into();
        let maxz: [f32; 4] = self.max.z.into();
        [
            Aabb3::new_unchecked([minx[0], miny[0], minz[0]], [maxx[0], maxy[0], maxz[0]]),
            Aabb3::new_unchecked([minx[1], miny[1], minz[1]], [maxx[1], maxy[1], maxz[1]]),
            Aabb3::new_unchecked([minx[2], miny[2], minz[2]], [maxx[2], maxy[2], maxz[2]]),
            Aabb3::new_unchecked([minx[3], miny[3], minz[3]], [maxx[3], maxy[3], maxz[3]]),
        ]
    }
}

#[cfg(test)]
mod tests {
    use std::f32::consts::PI;

    use approx_det::assert_relative_eq;
    use glam_det::nums::{f32x4, NumConstEx};

    use super::*;

    #[test]
    fn test_aabb_new() {
        let _ = env_logger::builder().is_test(true).try_init();

        let f1 = f32x4::ONE;
        let f2 = f32x4::const_splat(2.0);
        let f3 = f32x4::const_splat(3.0);
        let aabb = Aabb3Wide::new(Point3x4::new(f1, -f2, f3), Point3x4::new(-f1, f2, -f3));
        assert_relative_eq!(aabb.max(), Point3x4::new(f1, f2, f3));
        let p0 = aabb.decompose()[0];
        let max: [f32; 3] = p0.max().into();
        assert_relative_eq!(&max[..], &[1f32, 2f32, 3f32][..]);
        let aabb2 =
            Aabb3Wide::new_unchecked(Point3x4::new(f1, -f2, f3), Point3x4::new(-f1, f2, -f3));
        assert_relative_eq!(aabb2.max(), Point3x4::new(-f1, f2, -f3));
        let aabb3 = Aabb3Wide::from_array_unchecked([f1, -f2, f3, -f1, f2, -f3]);
        assert_relative_eq!(aabb3.max(), Point3x4::new(-f1, f2, -f3));
        let aabb4 = Aabb3Wide::ZERO;
        assert_relative_eq!(aabb4.max(), Point3x4::ZERO);
        let aabb5 = Aabb3Wide::ANTI_INFINITE;
        assert!(aabb5.min().extract_lane(0).is_finite());
    }

    #[test]
    fn test_aabb_grow() {
        let _ = env_logger::builder().is_test(true).try_init();

        let mut aabb = Aabb3Wide::new(Point3x4::ZERO, Point3x4::X);
        aabb.grow(Point3x4::new(
            f32x4::const_splat(-1.0f32),
            f32x4::const_splat(2f32),
            f32x4::const_splat(3f32),
        ));
        assert_relative_eq!(aabb.min(), Point3x4::NEG_X);
        assert_relative_eq!(
            aabb.max(),
            Point3x4::new(
                f32x4::const_splat(1f32),
                f32x4::const_splat(2f32),
                f32x4::const_splat(3f32)
            )
        );
    }

    #[test]
    fn test_aabb_corners() {
        let _ = env_logger::builder().is_test(true).try_init();

        let f1 = f32x4::ONE;
        let f2 = f32x4::const_splat(2.0);
        let f3 = f32x4::const_splat(3.0);
        let aabb = Aabb3Wide::new(Point3x4::new(-f1, -f2, -f3), Point3x4::new(f1, f2, f3));
        let corners = aabb.corners();
        assert_relative_eq!(corners[0], Point3x4::new(-f1, -f2, -f3));
        assert_relative_eq!(corners[1], Point3x4::new(-f1, -f2, f3));
        assert_relative_eq!(corners[2], Point3x4::new(-f1, f2, -f3));
        assert_relative_eq!(corners[3], Point3x4::new(-f1, f2, f3));

        assert_relative_eq!(corners[4], Point3x4::new(f1, -f2, -f3));
        assert_relative_eq!(corners[5], Point3x4::new(f1, -f2, f3));
        assert_relative_eq!(corners[6], Point3x4::new(f1, f2, -f3));

        assert_relative_eq!(corners[7], Point3x4::new(f1, f2, f3));
    }

    #[test]
    fn test_transform() {
        let _ = env_logger::builder().is_test(true).try_init();

        let f1 = f32x4::ONE;
        let f2 = f32x4::const_splat(2.0);
        let f3 = f32x4::const_splat(3.0);
        let aabb = Aabb3Wide::new(Point3x4::new(-f1, -f2, -f3), Point3x4::new(f1, f2, f3));
        let transform = Isometry3x4::from_rotation_x(f32x4::const_splat(PI / 2.0));
        let aabb_result = aabb.transform(transform);
        assert!(
            (aabb_result.extract(0).min() - phys_geom::math::Point3::new(-1f32, -3f32, -2f32))
                .norm()
                < 1e-6
        );
        assert!(
            (aabb_result.extract(0).max() - phys_geom::math::Point3::new(1f32, 3f32, 2f32)).norm()
                < 1e-6
        );
    }
}