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::*;
use glam_det::{Isometry3, Point3, Point3x4, Vec3, Vec3x4};
pub use phys_geom::shape::Cuboid;

use crate::traits::{
    ArrayGetter, BaseShapeWide, ContainsPoint, ContainsResult, CreateShapeWide, Expansion,
    MinkowskiSupport, MinkowskiSupportResult, MinkowskiSupportResultWide, MinkowskiSupportWide,
    SignedDistanceToPoint,
};
use crate::ShapeContainer;

const VERTICES: [Point3; 8] = [
    Point3::new(-1.0, 1.0, -1.0),
    Point3::new(1.0, 1.0, -1.0),
    Point3::new(-1.0, -1.0, -1.0),
    Point3::new(1.0, -1.0, -1.0),
    Point3::new(-1.0, 1.0, 1.0),
    Point3::new(1.0, 1.0, 1.0),
    Point3::new(-1.0, -1.0, 1.0),
    Point3::new(1.0, -1.0, 1.0),
];

pub trait CuboidExt {
    /// Get the vertex of the cuboid at the given index.
    /// The index must be in the range [0, 7].
    ///
    /// The vertices are ordered as follows:
    ///
    /// ```text
    /// 0: (-x, +y, -z)
    /// 1: (+x, +y, -z)
    /// 2: (-x, -y, -z)
    /// 3: (+x, -y, -z)
    /// 4: (-x, +y, +z)
    /// 5: (+x, +y, +z)
    /// 6: (-x, -y, +z)
    /// 7: (+x, -y, +z)
    /// ```
    ///
    /// The vertices are ordered in such a way that
    ///
    /// - the normal of the face formed by the vertices 0, 1, 2, 3 is (0, 0, -1).
    /// - The normal of the face formed by the vertices 4, 5, 6, 7 is (0, 0, 1).
    /// - The normal of the face formed by the vertices 0, 1, 4, 5 is (0, 1, 0).
    /// - The normal of the face formed by the vertices 2, 3, 6, 7 is (0, -1, 0).
    /// - The normal of the face formed by the vertices 0, 2, 4, 6 is (-1, 0, 0).
    /// - The normal of the face formed by the vertices 1, 3, 5, 7 is (1, 0, 0).
    ///
    /// # Panics
    /// Panics if the index is not in the range [0, 7].
    fn get_vertex(&self, index: usize) -> Point3;
}

impl CuboidExt for Cuboid {
    #[inline]
    fn get_vertex(&self, index: usize) -> Point3 {
        let [hx, hy, hz] = self.half_length();
        let point = VERTICES[index];
        Point3::new(point.x * hx, point.y * hy, point.z * hz)
    }
}

/// When calling the `support_point` function, if the direction changes from (0,0,1) to (-0,-0,-1)
/// , rust's `f32::signum()` function will return a different value because of -0, although it is
/// logically correct, But it takes a long time to go elsewhere for a suitable gjk algorithm.
#[inline]
fn sign(value: f32) -> f32 {
    if value > 0.0 {
        1.0
    } else {
        -1.0
    }
}

impl MinkowskiSupport for Cuboid {
    fn support_point(&self, direction: Vec3, transform: &Isometry3) -> MinkowskiSupportResult {
        let half_length = self.half_length();
        let dir = transform.inverse().transform_vector3(direction);
        let result = Point3::new(
            sign(dir.x) * half_length[0],
            sign(dir.y) * half_length[1],
            sign(dir.z) * half_length[2],
        );
        MinkowskiSupportResult {
            point: transform.transform_point3(result),
            point_index: 0,
        }
    }
}

impl MinkowskiSupportWide for CuboidWide {
    fn support_point_local(
        &self,
        local_direction: Vec3x4,
        _: Option<&ShapeContainer>,
    ) -> MinkowskiSupportResultWide {
        let p = Point3x4::new(
            local_direction.x.signumf() * self.half_length.x,
            local_direction.y.signumf() * self.half_length.y,
            local_direction.z.signumf() * self.half_length.z,
        );
        MinkowskiSupportResultWide {
            point: p,
            point_index: u32x4::ZERO,
        }
    }
}

impl Expansion for Cuboid {
    #[inline]
    fn max_radius_and_max_angular_expansion(&self) -> (f32, f32) {
        let half_length = Vec3::from_array(self.half_length());
        let max_radius = half_length.length();
        (max_radius, max_radius - half_length.min_element())
    }
}

impl ContainsPoint for Cuboid {
    #[inline]
    fn contains_point_with_threshold(&self, local_point: Point3, threshold: f32) -> ContainsResult {
        let half_length: Vec3 = self.half_length().into();
        let v = local_point.as_vec3().abs();
        let bounds_to_point = v - half_length;
        let distance = bounds_to_point.max_element();

        if distance.absf() < threshold {
            ContainsResult::Surface
        } else if distance < 0.0 {
            ContainsResult::Inside
        } else {
            ContainsResult::Outside
        }
    }
}

impl SignedDistanceToPoint for Cuboid {
    fn signed_distance_to_point(&self, local_point: Point3) -> f32 {
        let half_length: Vec3 = self.half_length().into();
        let v = local_point.as_vec3().abs();
        let bounds_to_point = v - half_length;

        // Refer to https://iquilezles.org/articles/distfunctions/
        bounds_to_point.max(Vec3::ZERO).length() + bounds_to_point.max_element().min(0.0)
    }
}

#[derive(Debug)]
pub struct CuboidWide {
    pub half_length: Vec3x4,
}

impl Default for CuboidWide {
    #[inline]
    fn default() -> Self {
        Self {
            half_length: Vec3x4::ZERO,
        }
    }
}

impl BaseShapeWide for CuboidWide {
    type TShape = Cuboid;
}

macro_rules! impl_cuboid_wide {
    ($($num:tt),*) => {
            $(
                impl CreateShapeWide<$num> for CuboidWide {
                    fn create<'a>(iter: impl Iterator<Item=&'a Self::TShape>) -> Self where Self::TShape: 'a {
                        Self {
                            half_length: ArrayGetter::<$num>::get_vec3x4_from_array4(ArrayGetter::<$num>::get_array4_from_iter(iter.map(#[inline]|shape| {
                                Vec3::from(shape.half_length())
                            }), Vec3::ZERO)),
                        }
                    }
                }
            )*
    };
}

impl_cuboid_wide!(1, 2, 3, 4);

#[cfg(test)]
mod tests {
    use approx_det::assert_relative_eq;
    use wasm_bindgen_test::*;

    use super::*;
    use crate::Shape;

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

        let container = ShapeContainer::default();
        let cuboid = Shape::Cuboid(Cuboid::new(Vec3::ONE * 2.0)).into_shape_ref(&container);

        assert_eq!(
            cuboid.contains_point(Point3::new(0.0, 0.0, 0.0)),
            ContainsResult::Inside
        );

        let points_on_surface = vec![
            Point3::Y,
            Point3::Z,
            Point3::X,
            Point3::NEG_Y,
            Point3::NEG_Z,
            Point3::NEG_X,
            Point3::X + Vec3::Y + Vec3::Z,
            Point3::X + Vec3::Y - Vec3::Z,
            Point3::X - Vec3::Y + Vec3::Z,
            Point3::X - Vec3::Y - Vec3::Z,
            -Point3::X + Vec3::Y + Vec3::Z,
            -Point3::X + Vec3::Y - Vec3::Z,
            -Point3::X - Vec3::Y + Vec3::Z,
            -Point3::X - Vec3::Y - Vec3::Z,
        ];

        for point in &points_on_surface {
            assert_eq!(
                cuboid.contains_point(*point),
                ContainsResult::Surface,
                "point: {point}"
            );
        }

        for point in &points_on_surface {
            assert_eq!(
                cuboid.contains_point(Point3::from_vec3(point.as_vec3() * Vec3::splat(0.99))),
                ContainsResult::Inside,
                "point: {point}"
            );
        }

        for point in &points_on_surface {
            assert_eq!(
                cuboid.contains_point(Point3::from_vec3(point.as_vec3() * Vec3::splat(1.01))),
                ContainsResult::Outside,
                "point: {point}"
            );
        }

        // surface
        for point in &points_on_surface {
            assert_relative_eq!(cuboid.signed_distance_to_point(*point), 0.0);
        }

        // outer
        for orignal_point in &points_on_surface {
            let point = Point3::from_vec3(orignal_point.as_vec3() * Vec3::splat(1.1));
            let distance = point.distance(*orignal_point);
            assert_relative_eq!(cuboid.signed_distance_to_point(point), distance);
        }

        // inner
        for orignal_point in points_on_surface.iter().take(6) {
            let point = Point3::from_vec3(orignal_point.as_vec3() * Vec3::splat(0.9));
            let distance = point.distance(*orignal_point);
            assert_relative_eq!(cuboid.signed_distance_to_point(point), -distance);
        }
    }

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

        let cuboid = Cuboid::new(Vec3::ONE * 2.0);
        let (max_radius, max_angular_expansion) = cuboid.max_radius_and_max_angular_expansion();
        assert_relative_eq!(max_radius, 1.0 * 3f32.sqrt());
        assert_relative_eq!(max_angular_expansion, 1.0 * 3f32.sqrt() - 1f32);
    }
}

#[cfg(test)]
mod minkowski_support_wide_cuboid_tests {
    use glam_det::nums::f32x4;
    use glam_det::nums::num_traits::{Bool, Num};
    use wasm_bindgen_test::wasm_bindgen_test;

    use super::*;

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

        let cuboid_1 = Cuboid::new(Vec3::new(1.2, 1.0, 1.6));
        let cuboid_2 = Cuboid::new(Vec3::new(1.0, 2.0, 3.0));
        let cuboid_3 = Cuboid::new(Vec3::new(1.0, 2.0, 3.0));
        let cuboid_4 = Cuboid::new(Vec3::new(10.0, 10.0, 10.0));
        let direction_wide = Vec3x4::new(
            f32x4::from([1.0, -3.0, 0.0, -1.0]),
            f32x4::from([1.0, 0.1, -0.1, -1.0]),
            f32x4::from([1.0, -4.0, 0.0, -1.0]),
        );
        let cuiboid_wide = <CuboidWide as CreateShapeWide<4>>::create(
            [cuboid_1, cuboid_2, cuboid_3, cuboid_4].iter(),
        );

        let support_point_wide = cuiboid_wide.support_point_local(direction_wide, None);
        let correct_result = MinkowskiSupportResultWide {
            point: Point3x4::new(
                f32x4::from([1.2, -1.0, 1.0, -10.0]) / f32x4::TWO,
                f32x4::from([1.0, 2.0, -2.0, -10.0]) / f32x4::TWO,
                f32x4::from([1.6, -3.0, 3.0, -10.0]) / f32x4::TWO,
            ),
            point_index: u32x4::ZERO,
        };
        assert!(
            (support_point_wide.point.as_vec3x4() - correct_result.point.as_vec3x4())
                .cmplt(Vec3x4::splat(f32x4::splat(1e-7)))
                .all()
                .all()
        );
        assert_eq!(support_point_wide.point_index, correct_result.point_index);
    }
}