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

use super::sphere::sphere_capsule_overlap_test;
use super::traits::OverlapTest;
use crate::geometric_tools::distance_squared_between_two_segments;
use crate::minkowski::gjk_intersection;
use crate::{Capsule, ConvexHull, Cuboid, Cylinder, InfinitePlane, ShapeContainer, Sphere};

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

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

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

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

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

impl OverlapTest<ConvexHull> for Capsule {
    #[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 capsule_infinite_plane_overlap_test(
    a: Capsule,
    _: InfinitePlane,
    offset_infinite_plane_to_capsule: Vec3,
    orientation_a: UnitQuat,
    orientation_b: UnitQuat,
) -> bool {
    let capsule_in_b = orientation_b.inverse() * offset_infinite_plane_to_capsule;
    let capsule_axis_in_b = orientation_b.inverse() * orientation_a * UnitVec3::Y;
    let top_center = capsule_in_b + capsule_axis_in_b * a.half_height();
    let bottom_center = capsule_in_b + capsule_axis_in_b * -a.half_height();

    top_center.y <= a.radius() || bottom_center.y <= a.radius()
}

pub fn capsule_cylinder_overlap_test(
    a: Capsule,
    b: Cylinder,
    offset_cylinder_to_capsule: Vec3,
    orientation_a: UnitQuat,
    orientation_b: UnitQuat,
) -> bool {
    const MAX_ITERATION_TIMES: usize = 13;
    // numerical solver to find mimimum distance between capsule segment and cylinder

    // consider point P on capsule axis segment P(t) = o + t * d, t is in range [-half_height,
    // half_height] (where d is direction of capsule axis, o is center of capsule)
    // The distance between point P(t) and cylinder is D(t), as cylinder is a convex shape, D(t) is
    // a concave function Then we use numerical solver to find the mimimum distance between
    // capsule segment and cylinder
    // 1. init t = 0, the range is [min, max], min = -half_height, max = half_height, t = (min +
    //    max) / 2
    // 2. find closest point Q(t) on cylinder to minimum |PQ|
    // 3. project Q(t) to capsule segment, denote the projection point as R, and R = o + t_new * d
    // 4. if R is very close to P(t), |PQ| is the minimum distance
    // 5. otherwise the search range becomes [min, t_new] or [t_new, max], goto step 2
    // 6. set a maximum iteration times to break

    // capsule center in cylinder local space
    let capsule_center = orientation_b.inverse() * offset_cylinder_to_capsule;
    // capsule axis direction in cylinder local space
    let capsule_direction = orientation_b.inverse() * orientation_a * Vec3::Y;

    let mut min = -a.half_height();
    let mut max = a.half_height();
    let mut t = 0.0;
    // point P(t) on capsule axis segment is (capsule_center + t * capsule_direction)
    let epsilon = a.half_height() * 1e-7;
    let cylinder_radius_squared = b.radius() * b.radius();
    let capsule_radius_squared = a.radius() * a.radius();
    for _ in 0..MAX_ITERATION_TIMES {
        // find closest point on cylinder to point P(t)
        let p = capsule_center + t * capsule_direction;
        // project point P(t) to cylinder plane y=0
        let proj_xz_distance_squared = p.x * p.x + p.z * p.z;
        let closest_y = p.y.clamp(-b.half_height(), b.half_height());
        let closest_on_cylinder = if proj_xz_distance_squared > cylinder_radius_squared {
            let scale = b.radius() / proj_xz_distance_squared.sqrtf();
            Vec3::new(p.x * scale, closest_y, p.z * scale)
        } else {
            Vec3::new(p.x, closest_y, p.z)
        };

        let t_projected = (closest_on_cylinder - capsule_center)
            .dot(capsule_direction)
            .clamp(min, max);
        if (p - closest_on_cylinder).length_squared() <= capsule_radius_squared {
            return true;
        }
        let delta_t = t_projected - t;
        if delta_t.absf() < epsilon {
            break;
        }
        if delta_t > 0.0 {
            min = t_projected;
        } else {
            max = t_projected;
        }
        t = (min + max) * 0.5_f32;
    }
    false
}

pub fn capsule_capsule_overlap_test(
    a: Capsule,
    b: Capsule,
    offset_b: Vec3,
    orientation_a: UnitQuat,
    orientation_b: UnitQuat,
) -> bool {
    let direction_a = orientation_a * UnitVec3::Y;
    let direction_b = orientation_b * UnitVec3::Y;

    // b - a == offset_b
    // (b - direction_b * half_height_b) - (a - direction_a * half_height_a) == offset_start
    // offset_start == offset_b - direction_b * half_height_b + direction_a * half_height_a
    let offset_start = offset_b - direction_b * b.half_height() + direction_a * a.half_height();
    let distance_squared = distance_squared_between_two_segments(
        offset_start,
        direction_a,
        a.height(),
        direction_b,
        b.height(),
    );
    let sum_radius = a.radius() + b.radius();

    distance_squared <= sum_radius * sum_radius
}

#[inline]
pub fn capsule_cuboid_overlap_test(
    capsule: Capsule,
    cuboid: Cuboid,
    offset_b: Vec3,
    orientation_a: UnitQuat,
    orientation_b: UnitQuat,
) -> bool {
    gjk_intersection(
        &capsule,
        Isometry3::from_quat(orientation_a),
        &cuboid,
        Isometry3::from_rotation_translation(orientation_b, offset_b),
        offset_b,
        0.0001f32,
    )
    .0
}

#[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_capsule_capsule() {
        let _ = env_logger::builder().is_test(true).try_init();

        let a = Capsule::new(1.0, 1.0);
        let b = a;

        assert!(capsule_capsule_overlap_test(
            a,
            b,
            Vec3::new(2.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY
        ));

        assert!(!capsule_capsule_overlap_test(
            a,
            b,
            Vec3::new(2.1, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY
        ));

        assert!(capsule_capsule_overlap_test(
            a,
            b,
            Vec3::new(0.0, 0.0, 3.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2)
        ));

        assert!(!capsule_capsule_overlap_test(
            a,
            b,
            Vec3::new(0.0, 0.0, 3.1),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2)
        ));

        assert!(capsule_capsule_overlap_test(
            a,
            b,
            Vec3::new(2.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2)
        ));

        assert!(!capsule_capsule_overlap_test(
            a,
            b,
            Vec3::new(2.1, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2)
        ));
    }

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

        let cuboid = Cuboid::new_xyz(2.0, 2.0, 2.0);
        let capsule = Capsule::new(1.0, 1.0);

        assert!(capsule_cuboid_overlap_test(
            capsule,
            cuboid,
            Vec3::new(2.0, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY
        ));

        assert!(!capsule_cuboid_overlap_test(
            capsule,
            cuboid,
            Vec3::new(2.1, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY
        ));

        assert!(capsule_cuboid_overlap_test(
            capsule,
            cuboid,
            Vec3::new(0.0, 3.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY
        ));

        assert!(!capsule_cuboid_overlap_test(
            capsule,
            cuboid,
            Vec3::new(0.0, 3.1, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY
        ));

        assert!(capsule_cuboid_overlap_test(
            capsule,
            cuboid,
            Vec3::new(0.0, 2.0, 0.0),
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            UnitQuat::IDENTITY
        ));

        assert!(!capsule_cuboid_overlap_test(
            capsule,
            cuboid,
            Vec3::new(0.0, 2.1, 0.0),
            UnitQuat::from_axis_angle(UnitVec3::X, FRAC_PI_2),
            UnitQuat::IDENTITY
        ));
    }

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

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

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

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

        // seperate, capsule side close with cylinder cap
        assert!(!capsule.overlap_test(
            &cylinder,
            Vec3::new(0.0, 2.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
        ));

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

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

        // cylinder side cross capsule cap
        assert!(capsule.overlap_test(
            &cylinder,
            Vec3::new(0.0, 0.1, 1.2),
            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
        ));

        // cylinder side cross capsule side
        assert!(capsule.overlap_test(
            &cylinder,
            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!(capsule.overlap_test(
            &cylinder,
            Vec3::new(0.1, 3.0, 0.1),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

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

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

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

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

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

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

        // derived from capsule convex tests
        let capsule = Capsule::new(1.0, 1.0);
        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);

        let a = Shape::Capsule(capsule).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.001, 0.0, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::IDENTITY,
            None
        ));

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

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

        assert!(overlap(
            &a,
            &b,
            vec3(1.773, 1.773, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(0.0, 0.0, 0.38268, 0.92388),
            None
        ));

        assert!(overlap(
            &a,
            &b,
            vec3(1.483, 1.483, 0.0),
            UnitQuat::IDENTITY,
            UnitQuat::from_xyzw_unchecked(0.0, 0.0, 0.38268, 0.92388),
            None
        ));

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

        assert!(overlap(
            &a,
            &b,
            vec3(0.0, 0.0, 2.0),
            UnitQuat::from_xyzw_unchecked(0.50218, 0.50218, 0.49781, 0.49781),
            UnitQuat::IDENTITY,
            None
        ));
    }
}