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::{Float, Signed};
use glam_det::{Isometry3, Point3, UnitQuat, Vec2, Vec3};

use super::traits::OverlapTest;
use crate::minkowski::gjk_intersection;
use crate::{Capsule, ConvexHull, Cuboid, Cylinder, InfinitePlane, ShapeContainer, Sphere};
pub fn sphere_sphere_overlap_test(sphere_a: Sphere, sphere_b: Sphere, offset_b: Vec3) -> bool {
    let radius_sum = sphere_a.radius() + sphere_b.radius();
    offset_b.length_squared() <= radius_sum * radius_sum
}

pub fn sphere_capsule_overlap_test(
    sphere: Sphere,
    capsule: Capsule,
    offset_capsule_to_sphere: Vec3,
    orientation_capsule: UnitQuat,
) -> bool {
    let point = Point3::from_vec3(orientation_capsule.inverse() * offset_capsule_to_sphere);

    // The following calculations occur in the local space of the capsule.
    let distance_squared = if point.y > capsule.half_height() {
        point.distance_squared(Point3::new(0.0, capsule.half_height(), 0.0))
    } else if point.y < -capsule.half_height() {
        point.distance_squared(Point3::new(0.0, -capsule.half_height(), 0.0))
    } else {
        offset_capsule_to_sphere.length_squared() - point.y * point.y
    };

    let radius_sum = sphere.radius() + capsule.radius();
    distance_squared <= radius_sum * radius_sum
}

pub fn sphere_cuboid_overlap_test(
    sphere: Sphere,
    cuboid: Cuboid,
    offset_cuboid_to_sphere: Vec3,
    orientation_cuboid: UnitQuat,
) -> bool {
    let point = orientation_cuboid.inverse() * offset_cuboid_to_sphere;

    let half_length: Vec3 = cuboid.half_length().into();

    // Transform the problem into the first octant
    let point_in_first_octant = point.abs();

    let distance_to_border = (point_in_first_octant - half_length).max(Vec3::splat(0.0));
    distance_to_border.length_squared() <= sphere.radius() * sphere.radius()
}

pub(super) fn sphere_cylinder_overlap_test(
    sphere: Sphere,
    cylinder: Cylinder,
    offset_cylinder_to_sphere: Vec3,
    orientation_cylinder: UnitQuat,
) -> bool {
    // find the closest point on the cylinder to the sphere center
    // then judge the distance

    // sphere center in cylinder local space
    let sphere_center = orientation_cylinder.inverse() * offset_cylinder_to_sphere;

    let proj_xz_length_squared =
        sphere_center.x * sphere_center.x + sphere_center.z * sphere_center.z;
    if proj_xz_length_squared < 1e-6 {
        // sphere center is on the cylinder axis
        let half_height = cylinder.half_height();
        return sphere_center.y.absf() <= half_height + sphere.radius();
    }
    let proj_xz_unit = Vec2::new(sphere_center.x, sphere_center.z).normalize();
    let closest_xz = proj_xz_unit * (cylinder.radius().min(proj_xz_length_squared.sqrtf()));
    let closest_y = sphere_center
        .y
        .clamp(-cylinder.half_height(), cylinder.half_height());
    let closest_point = Vec3::new(closest_xz.x, closest_y, closest_xz.y);

    (closest_point - sphere_center).length_squared() <= sphere.radius() * sphere.radius()
}

#[inline]
pub(super) fn sphere_infinite_plane_overlap_test(
    sphere: Sphere,
    _: InfinitePlane,
    offset_infinite_plane_to_sphere: Vec3,
    orientation_infinite_plane: UnitQuat,
) -> bool {
    let sphere_in_plane = orientation_infinite_plane.inverse() * offset_infinite_plane_to_sphere;
    sphere_in_plane.y <= sphere.radius()
}

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

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

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

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

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

impl OverlapTest<ConvexHull> for Sphere {
    #[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.0003f32, // for some tangent hit situations
        )
        .0
    }
}
#[cfg(test)]
mod tests {
    use std::f32::consts::FRAC_PI_2;

    use glam_det::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_sphere_sphere() {
        let _ = env_logger::builder().is_test(true).try_init();

        let a = Sphere::new(1.0);
        let b = a;
        assert!(sphere_sphere_overlap_test(a, b, Vec3::new(2.0, 0.0, 0.0)));
        assert!(!sphere_sphere_overlap_test(a, b, Vec3::new(2.1, 0.0, 0.0)));
    }

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

        let sphere = Sphere::new(1.0);
        let capsule = Capsule::new(1.0, 1.0);
        assert!(sphere_capsule_overlap_test(
            sphere,
            capsule,
            Vec3::new(0.0, 0.0, 2.0),
            UnitQuat::IDENTITY
        ));

        assert!(sphere_capsule_overlap_test(
            sphere,
            capsule,
            Vec3::new(0.0, 2.0, 0.0),
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2)
        ));

        assert!(sphere_capsule_overlap_test(
            sphere,
            capsule,
            Vec3::new(0.0, 2.0, 0.0),
            UnitQuat::IDENTITY
        ));

        assert!(!sphere_capsule_overlap_test(
            sphere,
            capsule,
            Vec3::new(0.0, 3.1, 0.0),
            UnitQuat::IDENTITY
        ));

        assert!(sphere_capsule_overlap_test(
            sphere,
            capsule,
            Vec3::new(0.0, -2.0, 0.0),
            UnitQuat::IDENTITY
        ));

        assert!(!sphere_capsule_overlap_test(
            sphere,
            capsule,
            Vec3::new(0.0, -3.1, 0.0),
            UnitQuat::IDENTITY
        ));

        assert!(!sphere_capsule_overlap_test(
            sphere,
            capsule,
            Vec3::new(0.0, 2.1, 0.0),
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2)
        ));

        assert!(!sphere_capsule_overlap_test(
            sphere,
            capsule,
            Vec3::new(0.0, 0.0, 2.1),
            UnitQuat::IDENTITY
        ));
    }

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

        let sphere = Sphere::new(1.0);
        let cuboid = Cuboid::new_xyz(2.0, 2.0, 2.0);
        assert!(sphere_cuboid_overlap_test(
            sphere,
            cuboid,
            Vec3::new(0.0, 0.0, 2.0),
            UnitQuat::IDENTITY
        ));
    }

    #[test]
    #[wasm_bindgen_test]
    fn test_sphere_cylinder() {
        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!(sphere.overlap_test(
            &cylinder,
            Vec3::new(0.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

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

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

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

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

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

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

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

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

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

        let sphere = Sphere::new(2.0f32);
        // cube as convex
        let cube = Cuboid::new_xyz(4.0, 4.0, 4.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);

        let a = Shape::Sphere(sphere).into_shape_ref(&container);
        let b = Shape::ConvexHull(convex_hull_id).into_shape_ref(&container);
        // sphere tangent the face
        assert!(overlap(
            &a,
            &b,
            Vec3::new(4.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // sphere overlap the face
        assert!(overlap(
            &a,
            &b,
            Vec3::new(3.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // sphere overlap the edge
        assert!(overlap(
            &a,
            &b,
            Vec3::new(3.414, 3.414, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // sphere overlap the vertex
        assert!(overlap(
            &a,
            &b,
            Vec3::new(2.3, 2.3, 2.3),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

        // no overlap
        assert!(!overlap(
            &a,
            &b,
            Vec3::new(4.001, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));
    }
}