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::{Isometry3, UnitQuat, Vec3};

use super::capsule::capsule_cylinder_overlap_test;
use super::infinite_plane::cylinder_infinite_plane_overlap_test;
use super::sphere::sphere_cylinder_overlap_test;
use super::traits::OverlapTest;
use crate::minkowski::gjk_intersection;
use crate::{Capsule, Cuboid, Cylinder, InfinitePlane, ShapeContainer, Sphere};

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

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

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

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

#[cfg(test)]
mod tests {
    use glam_det::UnitVec3;
    use wasm_bindgen_test::*;

    use super::*;

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

        let sphere = Sphere::new(1.0);
        let cylinder = Cylinder::new(1.0, 1.0);

        // same center
        assert!(cylinder.overlap_test(
            &sphere,
            Vec3::new(0.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // sphere on cylinder local y axis
        assert!(cylinder.overlap_test(
            &sphere,
            Vec3::new(0.0, 1.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        assert!(!cylinder.overlap_test(
            &sphere,
            Vec3::new(0.0, 2.1, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // closest point on cylinder is on the side
        assert!(!cylinder.overlap_test(
            &sphere,
            Vec3::new(2.0, -0.1, 2.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));
        assert!(cylinder.overlap_test(
            &sphere,
            Vec3::new(0.1, -0.1, 0.1),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // closest point on cylinder is on the top disk internal
        assert!(cylinder.overlap_test(
            &sphere,
            Vec3::new(0.1, 1.8, 0.2),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));
        assert!(!cylinder.overlap_test(
            &sphere,
            Vec3::new(0.1, 2.1, 0.2),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // closest point on cylinder is on the top disk circle
        assert!(!cylinder.overlap_test(
            &sphere,
            Vec3::new(2.0, 3.0, 3.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));
        assert!(cylinder.overlap_test(
            &sphere,
            Vec3::new(1.1, 1.0, 0.1),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // sphere tangent with cylinder bottom
        assert!(cylinder.overlap_test(
            &sphere,
            Vec3::new(0.1, 1.0, 0.1),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // sphere tangent with cylinder side
        assert!(cylinder.overlap_test(
            &sphere,
            Vec3::new(1.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // sphere tangent with cylinder bottom circle
        assert!(cylinder.overlap_test(
            &sphere,
            Vec3::new(1.0, 1.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));
    }

    #[test]
    #[wasm_bindgen_test]
    fn test_cylinder_cuboid() {
        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_cylinder_cuboid(
            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_cylinder_cuboid(
                cuboid,
                cylinder,
                *center,
                Vec3::new(0.0, 1.0, 0.0),
                true,
            );
            symmetrical_test_cylinder_cuboid(
                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_cylinder_cuboid(
                    cuboid,
                    cylinder,
                    input_center,
                    Vec3::new(0.0, 1.0, 0.0),
                    true,
                );
                symmetrical_test_cylinder_cuboid(
                    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_cylinder_cuboid(
                    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_cylinder_cuboid(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_cylinder_cuboid(
            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_cylinder_cuboid(cuboid, cylinder, input_center, *dir, true);
                }
            }
        }
    }

    fn symmetrical_test_cylinder_cuboid(
        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,
                cylinder.overlap_test(
                    &cuboid,
                    -cylinder_center,
                    cylinder_rotation,
                    UnitQuat::IDENTITY,
                    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_cylinder_capsule() {
        let _ = env_logger::builder().is_test(true).try_init();

        let cylinder = Cylinder::new(1.0, 1.0);
        let capsule = Capsule::new(1.0, 1.0);

        // same center
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // same center with rotation
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, 0.0, 0.0),
            UnitQuat::from_euler_default(-10.0, -20.0, 45.0),
            UnitQuat::from_euler_default(30.0, 40.0, 50.0),
            None
        ));

        // seperate, axis parallel
        assert!(!cylinder.overlap_test(
            &capsule,
            Vec3::new(-2.1, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // seperate, capsule axis same with cylinder axis
        assert!(!cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, 0.0, -5.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // seperate, capsule side close with cylinder cap
        assert!(!cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, -2.1, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(
                std::f32::consts::FRAC_1_SQRT_2,
                0.0,
                0.0,
                std::f32::consts::FRAC_1_SQRT_2
            )
            .renormalize(),
            None
        ));

        // cylinder cap cross capsule cap
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.2, -2.5, 0.1),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.2, 2.5, 0.1),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // cylinder side cross capsule side
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.2, -0.1, 0.1),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // cylinder side cross capsule cap
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, -0.1, 1.2),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(
                std::f32::consts::FRAC_1_SQRT_2,
                0.0,
                0.0,
                std::f32::consts::FRAC_1_SQRT_2
            )
            .renormalize(),
            None
        )); // cylinder side cross capsule side
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, -0.1, 0.0),
            UnitQuat::from_xyzw_unchecked(
                std::f32::consts::FRAC_1_SQRT_2,
                0.0,
                0.0,
                std::f32::consts::FRAC_1_SQRT_2
            )
            .renormalize(),
            UnitQuat::IDENTITY,
            None
        ));
        // capsule cap tangent with cylinder bottom
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(-0.1, -3.0, 0.1),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // capsule cap tangent with cylinder side
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(-3.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(
                0.0,
                0.0,
                std::f32::consts::FRAC_1_SQRT_2,
                std::f32::consts::FRAC_1_SQRT_2
            )
            .renormalize(),
            None
        ));

        // capsule cap tangent with cylinder bottom circle
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(-3.0, -1.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(
                0.0,
                0.0,
                std::f32::consts::FRAC_1_SQRT_2,
                std::f32::consts::FRAC_1_SQRT_2
            )
            .renormalize(),
            None
        ));

        // capsule side tangent with cylinder bottom
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, -1.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(
                0.0,
                0.0,
                std::f32::consts::FRAC_1_SQRT_2,
                std::f32::consts::FRAC_1_SQRT_2
            )
            .renormalize(),
            None
        ));

        // capsule side tangent with cylinder bottom circle
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, 1.0, 2.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(
                0.0,
                0.0,
                std::f32::consts::FRAC_1_SQRT_2,
                std::f32::consts::FRAC_1_SQRT_2
            )
            .renormalize(),
            None
        ));

        // capsule side tangent with cylinder side
        assert!(cylinder.overlap_test(
            &capsule,
            Vec3::new(0.0, 0.0, 2.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(
                0.0,
                0.0,
                std::f32::consts::FRAC_1_SQRT_2,
                std::f32::consts::FRAC_1_SQRT_2
            )
            .renormalize(),
            None
        ));
    }
}