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::Signed;
use glam_det::{
    AbsExternal, CrossExternal, DotExternal, Isometry3, NormalizeAndLengthExternal, UnitQuat,
    UnitVec3, Vec3, Vec3OnlyX, Vec3OnlyY, Vec3OnlyZ,
};

use super::capsule::capsule_cuboid_overlap_test;
use super::sphere::sphere_cuboid_overlap_test;
use super::traits::OverlapTest;
use crate::minkowski::gjk_intersection;
use crate::{Capsule, ConvexHull, Cuboid, Cylinder, InfinitePlane, ShapeContainer, Sphere};

impl OverlapTest<Sphere> for Cuboid {
    #[inline]
    fn overlap_test(
        &self,
        target: &Sphere,
        offset_target: Vec3,
        orientation: UnitQuat,
        _: UnitQuat,
        _: Option<&ShapeContainer>,
    ) -> bool {
        sphere_cuboid_overlap_test(*target, *self, offset_target, orientation)
    }
}

impl OverlapTest<Capsule> for Cuboid {
    #[inline]
    fn overlap_test(
        &self,
        target: &Capsule,
        offset_target: Vec3,
        orientation: UnitQuat,
        orientation_target: UnitQuat,
        _: Option<&ShapeContainer>,
    ) -> bool {
        capsule_cuboid_overlap_test(
            *target,
            *self,
            -offset_target,
            orientation_target,
            orientation,
        )
    }
}

fn update_depth<T: NormalizeAndLengthExternal + DotExternal + CrossExternal + AbsExternal>(
    edge: &T,
    offset_target: Vec3,
    half_length_self: impl Into<Vec3>,
    half_length_target: impl Into<Vec3>,
    min_depth: &mut f32,
) {
    let half_length_self = half_length_self.into();
    let half_length_target = half_length_target.into();
    let (normal, len) = edge.normalize_and_length_ext();
    let depth = if len < f32::EPSILON {
        f32::NEG_INFINITY
    } else {
        let offset_on_normal = normal.dot_ext(offset_target).absf();
        let normal_abs = normal.abs_ext();
        let shape_contribute = normal_abs.dot_ext(half_length_self).absf()
            + normal_abs.dot_ext(half_length_target).absf();
        offset_on_normal - shape_contribute
    };
    *min_depth = min_depth.max(depth);
}

impl OverlapTest<Cuboid> for Cuboid {
    #[inline]
    fn overlap_test(
        &self,
        target: &Cuboid,
        offset_target: Vec3,
        orientation: UnitQuat,
        orientation_target: UnitQuat,
        _: Option<&ShapeContainer>,
    ) -> bool {
        match cuboid_pair_intersection_test(
            self,
            target,
            offset_target,
            orientation,
            orientation_target,
        ) {
            Ok(value) => value,
            Err(value) => {
                log::error!(
                    "cuboid_pair_intersection_test failed,maybe the rotation value is not invalid"
                );
                value
            }
        }
    }
}

fn cuboid_pair_intersection_test(
    target0: &Cuboid,
    target1: &Cuboid,
    offset_target: Vec3,
    orientation: UnitQuat,
    orientation_target: UnitQuat,
) -> Result<bool, bool> {
    let tolerance = 0.0001f32;
    let r_b_a = orientation.inverse() * orientation_target;
    let a_axis_x = Vec3OnlyX::new(1.0);
    let a_axis_y = Vec3OnlyY::new(1.0);
    let a_axis_z = Vec3OnlyZ::new(1.0);
    let b_axis_x = r_b_a * Vec3::X;
    let b_axis_y = r_b_a * Vec3::Y;
    let b_axis_z = r_b_a * Vec3::Z;
    let mut min_depth = f32::NEG_INFINITY;
    // ax cross bx
    {
        let edge = a_axis_x.cross_ext(b_axis_x);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // ax cross by
    {
        let edge = a_axis_x.cross_ext(b_axis_y);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // ax cross bz
    {
        let edge = a_axis_x.cross_ext(b_axis_z);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // ay cross bx
    {
        let edge = a_axis_y.cross_ext(b_axis_x);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // ay cross by
    {
        let edge = a_axis_y.cross_ext(b_axis_y);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // ay cross bz
    {
        let edge = a_axis_y.cross_ext(b_axis_z);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // az cross bx
    {
        let edge = a_axis_z.cross_ext(b_axis_x);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // az cross by
    {
        let edge = a_axis_z.cross_ext(b_axis_y);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // az cross bz
    {
        let edge = a_axis_z.cross_ext(b_axis_z);
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // ax
    {
        let edge = a_axis_x;
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // ay
    {
        let edge = a_axis_y;
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // az
    {
        let edge = a_axis_z;
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // bx
    {
        let edge = b_axis_x;
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // by
    {
        let edge = b_axis_y;
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }
    // bz
    {
        let edge = b_axis_z;
        update_depth(
            &edge,
            offset_target,
            target0.half_length(),
            target1.half_length(),
            &mut min_depth,
        );
    }

    if min_depth == f32::NEG_INFINITY {
        return Err(false);
    }
    Ok(min_depth < tolerance)
}

impl OverlapTest<Cylinder> for Cuboid {
    #[inline]
    fn overlap_test(
        &self,
        target: &Cylinder,
        offset_target: Vec3,
        orientation: UnitQuat,
        orientation_target: UnitQuat,
        _: Option<&ShapeContainer>,
    ) -> bool {
        gjk_intersection(
            self,
            Isometry3::from_quat(orientation),
            target,
            Isometry3::from_rotation_translation(orientation_target, offset_target),
            offset_target,
            0.0001f32,
        )
        .0
    }
}

impl OverlapTest<ConvexHull> for Cuboid {
    #[inline]
    fn overlap_test(
        &self,
        target: &ConvexHull,
        offset_target: Vec3,
        orientation: UnitQuat,
        orientation_target: UnitQuat,
        _: Option<&ShapeContainer>,
    ) -> bool {
        gjk_intersection(
            self,
            Isometry3::from_quat(orientation),
            target,
            Isometry3::from_rotation_translation(orientation_target, offset_target),
            offset_target,
            0.0001f32,
        )
        .0
    }
}

#[inline]
pub(crate) fn cuboid_infinite_plane_overlap_test(
    a: Cuboid,
    _: InfinitePlane,
    offset_infinite_plane_to_cuboid: Vec3,
    orientation_a: UnitQuat,
    orientation_b: UnitQuat,
) -> bool {
    let box_center_in_plane = orientation_b.inverse() * offset_infinite_plane_to_cuboid;
    let box_depth = box_center_in_plane.y;

    let plane_normal_in_box = orientation_a.inverse() * orientation_b * UnitVec3::Y;

    let [half_x, half_y, half_z] = a.half_length();
    let dx = plane_normal_in_box.x * half_x;
    let dy = plane_normal_in_box.y * half_y;
    let dz = plane_normal_in_box.z * half_z;

    let depth0 = dx + dy + dz + box_depth;
    let depth1 = dx + dy - dz + box_depth;
    let depth2 = dx - dy + dz + box_depth;
    let depth3 = dx - dy - dz + box_depth;
    let depth4 = -dx + dy + dz + box_depth;
    let depth5 = -dx + dy - dz + box_depth;
    let depth6 = -dx - dy + dz + box_depth;
    let depth7 = -dx - dy - dz + box_depth;

    depth0 <= f32::EPSILON
        || depth1 <= f32::EPSILON
        || depth2 <= f32::EPSILON
        || depth3 <= f32::EPSILON
        || depth4 <= f32::EPSILON
        || depth5 <= f32::EPSILON
        || depth6 <= f32::EPSILON
        || depth7 <= f32::EPSILON
}

impl OverlapTest<InfinitePlane> for Cuboid {
    #[inline]
    fn overlap_test(
        &self,
        target: &InfinitePlane,
        offset_target: Vec3,
        orientation: UnitQuat,
        orientation_target: UnitQuat,
        _: Option<&ShapeContainer>,
    ) -> bool {
        cuboid_infinite_plane_overlap_test(
            *self,
            *target,
            -offset_target,
            orientation,
            orientation_target,
        )
    }
}
#[cfg(test)]
mod tests {
    use std::f32::consts::FRAC_PI_2;

    use glam_det::{vec3, Point3, UnitVec3};
    use wasm_bindgen_test::*;

    use super::*;
    use crate::overlap::overlap;
    use crate::shapes::CuboidExt;
    use crate::{Shape, ShapeContainer};
    #[test]
    #[wasm_bindgen_test]
    fn test_cuboid_cuboid() {
        let _ = env_logger::builder().is_test(true).try_init();

        let a = Cuboid::new_xyz(2.0, 2.0, 2.0);
        let b = a;
        assert!(a.overlap_test(
            &b,
            vec3(2.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        assert!(!a.overlap_test(
            &b,
            vec3(2.1, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        assert!(a.overlap_test(
            &b,
            vec3(2.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            None
        ));

        assert!(!a.overlap_test(
            &b,
            vec3(2.1, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            None
        ));

        assert!(!a.overlap_test(
            &b,
            vec3(0.0, 0.0, 2.4),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            None
        ));

        assert!(!a.overlap_test(
            &b,
            vec3(0.0, 0.0, 2.5),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            None
        ));
    }

    fn symmetrical_test_cuboid_cylinder(
        cuboid: Cuboid,
        cylinder: Cylinder,
        cylinder_center: Vec3,
        cylinder_direction: Vec3,
        hit: bool,
    ) {
        let direction = cylinder_direction.normalize_to_unit();
        let mut cylinder_rotations = vec![];
        let sign_x = -1..=1;
        let sign_y = -1..=1;
        let sign_z = -1..=1;
        sign_x.zip(sign_y).zip(sign_z).for_each(|((x, y), z)| {
            if x == 0 && y == 0 && z == 0 {
                return;
            }
            let mut new_direction = direction;
            if x == -1 {
                new_direction.x = -new_direction.x;
            }
            if y == -1 {
                new_direction.y = -new_direction.y;
            }
            if z == -1 {
                new_direction.z = -new_direction.z;
            }
            let cylinder_rotation = UnitQuat::from_rotation_arc(UnitVec3::Y, new_direction);
            cylinder_rotations.push(cylinder_rotation);
        });
        for cylinder_rotation in cylinder_rotations {
            assert_eq!(
                hit,
                cuboid.overlap_test(
                    &cylinder,
                    cylinder_center,
                    UnitQuat::IDENTITY,
                    cylinder_rotation,
                    None
                ),
            );
        }
    }

    fn origin_symmetric_points(p: Vec3) -> Vec<Vec3> {
        vec![
            p,
            Vec3::new(-p.x, p.y, p.z),
            Vec3::new(p.x, -p.y, p.z),
            Vec3::new(p.x, p.y, -p.z),
            Vec3::new(-p.x, -p.y, p.z),
            Vec3::new(-p.x, p.y, -p.z),
            Vec3::new(p.x, -p.y, -p.z),
            Vec3::new(-p.x, -p.y, -p.z),
        ]
    }

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

        // when cylinder is smaller than cuboid
        let cylinder = Cylinder::new(1.0, 0.5);
        let cuboid = Cuboid::new(Vec3::new(2.0, 4.0, 6.0));

        // same center (cylinder inside cuboid)
        symmetrical_test_cuboid_cylinder(
            cuboid,
            cylinder,
            Vec3::new(0.0, 0.0, 0.0),
            Vec3::new(0.0, 1.0, 0.0),
            true,
        );

        // cylinder hit cuboid at side face
        for center in &[
            Vec3::new(0.0, 2.8, 0.0),
            Vec3::new(0.0, 0.0, 3.1),
            Vec3::new(1.2, 0.0, 0.0),
        ] {
            symmetrical_test_cuboid_cylinder(
                cuboid,
                cylinder,
                *center,
                Vec3::new(0.0, 1.0, 0.0),
                true,
            );
            symmetrical_test_cuboid_cylinder(
                cuboid,
                cylinder,
                -*center,
                Vec3::new(0.0, 1.0, 0.0),
                true,
            );
        }

        // cylinder hit cuboid edge
        for center in &[
            Vec3::new(1.0, 1.8, 0.0),
            Vec3::new(0.0, 1.8, 2.8),
            Vec3::new(1.0, 0.0, 2.8),
            Vec3::new(1.0, 1.8, 2.8),
        ] {
            let symetric_points = origin_symmetric_points(*center);
            for input_center in symetric_points {
                symmetrical_test_cuboid_cylinder(
                    cuboid,
                    cylinder,
                    input_center,
                    Vec3::new(0.0, 1.0, 0.0),
                    true,
                );
                symmetrical_test_cuboid_cylinder(
                    cuboid,
                    cylinder,
                    input_center,
                    Vec3::new(1.0, 1.0, 1.0),
                    true,
                );
            }
        }

        for center in &[
            Vec3::new(14.0, 16.0, 18.0), // cuboid is outside cylinder slab
            Vec3::new(6.0, 0.0, 0.0),    // cylinder at side of cuboid
            Vec3::new(0.0, 6.0, 0.0),
            Vec3::new(0.0, 0.0, 6.0),
            Vec3::new(6.0, 6.0, 0.0),
            Vec3::new(0.0, 6.0, 6.0),
            Vec3::new(6.0, 0.0, 6.0),
        ] {
            let symetric_points = origin_symmetric_points(*center);
            for input_center in symetric_points {
                symmetrical_test_cuboid_cylinder(
                    cuboid,
                    cylinder,
                    input_center,
                    Vec3::new(1.0, 1.0, 1.0),
                    false,
                );

                for dir in &[
                    Vec3::new(1.0, 1.0, 1.0),
                    Vec3::new(1.0, 0.0, 0.0),
                    Vec3::new(0.0, 1.0, 0.0),
                    Vec3::new(0.0, 0.0, 1.0),
                    Vec3::new(1.0, 1.0, 0.0),
                    Vec3::new(0.0, 1.0, 1.0),
                    Vec3::new(1.0, 0.0, 1.0),
                ] {
                    symmetrical_test_cuboid_cylinder(cuboid, cylinder, input_center, *dir, false);
                }
            }
        }

        // when cylinder is larger than cuboid
        let cuboid = Cuboid::new(Vec3::new(1.0, 2.0, 3.0));
        let cylinder = Cylinder::new(2.0, 2.0);
        // same center (cuboid inside cylinder)
        symmetrical_test_cuboid_cylinder(
            cuboid,
            cylinder,
            Vec3::new(0.0, 0.0, 0.0),
            Vec3::new(1.0, 1.0, 1.0),
            true,
        );

        // cylinder hit cuboid
        for center in &[
            Vec3::new(0.0, 0.0, 2.4),
            Vec3::new(0.0, 2.0, 0.0),
            Vec3::new(1.6, 0.0, 0.0),
            Vec3::new(1.6, 1.5, 0.0),
            Vec3::new(1.6, 0.8, 2.0),
            Vec3::new(0.7, 2.0, 2.0),
        ] {
            let symetric_points = origin_symmetric_points(*center);
            for input_center in symetric_points {
                for dir in &[
                    Vec3::new(1.0, 0.0, 0.0),
                    Vec3::new(0.0, 1.0, 0.0),
                    Vec3::new(0.0, 0.0, 1.0),
                    Vec3::new(1.0, 1.0, 0.0),
                    Vec3::new(0.0, 1.0, 1.0),
                    Vec3::new(1.0, 0.0, 1.0),
                    Vec3::new(1.0, 1.0, 1.0),
                ] {
                    symmetrical_test_cuboid_cylinder(cuboid, cylinder, input_center, *dir, true);
                }
            }
        }
    }

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

        let cube = Cuboid::new_xyz(2.0, 2.0, 2.0);
        let vertice_indexs = 0..8_usize;
        let vertices: Vec<Point3> = vertice_indexs
            .into_iter()
            .map(|i| cube.get_vertex(i))
            .collect();
        let convex_hull = ConvexHull::new_unchecked(&vertices);

        let mut container = ShapeContainer::default();
        let convex_hull_id = container.add(convex_hull);

        // copy from cuboid vs cuboid overlap test
        let a = Shape::Cuboid(cube).into_shape_ref(&container);
        let b = Shape::ConvexHull(convex_hull_id).into_shape_ref(&container);
        assert!(overlap(
            &a,
            &b,
            vec3(2.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        assert!(!overlap(
            &a,
            &b,
            vec3(2.1, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        assert!(overlap(
            &a,
            &b,
            vec3(2.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            None
        ));

        assert!(!overlap(
            &a,
            &b,
            vec3(2.1, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            None
        ));

        assert!(!overlap(
            &a,
            &b,
            vec3(0.0, 0.0, 2.4),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            None
        ));

        assert!(!overlap(
            &a,
            &b,
            vec3(0.0, 0.0, 2.5),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            None
        ));
    }
}