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

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

pub trait CapsuleExt {
    fn point_a(&self) -> Point3;
    fn point_b(&self) -> Point3;
}

impl CapsuleExt for Capsule {
    fn point_a(&self) -> Point3 {
        Point3::new(0.0, self.half_height(), 0.0)
    }

    fn point_b(&self) -> Point3 {
        Point3::new(0.0, -self.half_height(), 0.0)
    }
}

impl MinkowskiSupport for Capsule {
    fn support_point(&self, direction: Vec3, transform: &Isometry3) -> MinkowskiSupportResult {
        let direction = transform.inverse().transform_vector3(direction);
        let result = Point3::new(0.0, self.half_height() * direction.y.signumf(), 0.0)
            + direction.normalize() * self.radius();
        MinkowskiSupportResult {
            point: transform.transform_point3(result),
            point_index: 0,
        }
    }
}

impl ContainsPoint for Capsule {
    fn contains_point_with_threshold(&self, local_point: Point3, threshold: f32) -> ContainsResult {
        let radius = self.radius();
        let distance_squared = distance_squared_point_to_segment(
            local_point,
            self.point_a(),
            self.point_b(),
            self.height(),
        );

        let radius_squared = radius * radius;

        if (distance_squared - radius_squared).absf() < threshold {
            ContainsResult::Surface
        } else if distance_squared < radius_squared {
            ContainsResult::Inside
        } else {
            ContainsResult::Outside
        }
    }
}

impl SignedDistanceToPoint for Capsule {
    fn signed_distance_to_point(&self, local_point: Point3) -> f32 {
        // TODO: There may be a better way. (issue #1435)
        let distance_squared = distance_squared_point_to_segment(
            local_point,
            self.point_a(),
            self.point_b(),
            self.height(),
        );

        let distance = distance_squared.sqrtf();

        distance - self.radius()
    }
}

impl ConvexShape for Capsule {}

impl Expansion for Capsule {
    #[inline]
    fn max_radius_and_max_angular_expansion(&self) -> (f32, f32) {
        (self.radius() + self.half_height(), self.half_height())
    }
}

#[derive(Debug)]
pub struct CapsuleWide {
    pub half_height: f32x4,
    pub radius: f32x4,
}

impl Default for CapsuleWide {
    fn default() -> Self {
        Self {
            half_height: f32x4::ZERO,
            radius: f32x4::ZERO,
        }
    }
}
impl MinkowskiSupportWide for CapsuleWide {
    fn support_point_local(
        &self,
        local_direction: Vec3x4,
        _: Option<&ShapeContainer>,
    ) -> MinkowskiSupportResultWide {
        let point = Point3x4::new(
            f32x4::ZERO,
            self.half_height * local_direction.y.signumf(),
            f32x4::ZERO,
        ) + local_direction.normalize() * self.radius;
        MinkowskiSupportResultWide {
            point,
            point_index: u32x4::ZERO,
        }
    }
}

macro_rules! impl_capsule_wide {
    ($($num:tt),*) => {
        $(
          impl CreateShapeWide<$num> for CapsuleWide {
            fn create<'a>(iter: impl Iterator<Item=&'a Self::TShape> + Clone) -> Self where Self::TShape: 'a {
                Self {
                    half_height: f32x4::from(ArrayGetter::<$num>::get_array4_from_iter(iter.clone().map(#[inline]|capsule| {
                        capsule.half_height()
                    }), 0.0)),
                    radius: f32x4::from(ArrayGetter::<$num>::get_array4_from_iter(iter.clone().map(#[inline]|capsule| {
                        capsule.radius()
                    }), 0.0)),
                }
            }
          }
        )*
    }

}

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

impl BaseShapeWide for CapsuleWide {
    type TShape = Capsule;
}

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

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

    #[cfg(test)]
    mod signed_distance_tests {

        use approx_det::assert_relative_eq;

        use super::*;

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

            let container = ShapeContainer::default();
            let capsule = Shape::Capsule(Capsule::new(1.0, 1.0)).into_shape_ref(&container);

            // upper
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, 3.0, 0.0)),
                1.0
            );

            // side
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(2.0, 0.0, 0.0)),
                1.0
            );

            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(-2.0, 0.0, 0.0)),
                1.0
            );

            // bottom
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, -3.0, 0.0)),
                1.0
            );
        }

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

            let container = ShapeContainer::default();
            let capsule = Shape::Capsule(Capsule::new(1.0, 1.0)).into_shape_ref(&container);

            // segment
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, 0.0, 0.0)),
                -1.0
            );

            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, 1.0, 0.0)),
                -1.0
            );

            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, -1.0, 0.0)),
                -1.0
            );

            // upper
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, 1.5, 0.0)),
                -0.5
            );

            // bottom
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, -1.5, 0.0)),
                -0.5
            );

            // side
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.5, 0.0, 0.0)),
                -0.5
            );
        }

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

            let container = ShapeContainer::default();
            let capsule = Shape::Capsule(Capsule::new(1.0, 1.0)).into_shape_ref(&container);

            // upper
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, 2.0, 0.0)),
                0.0
            );

            // bottom
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(0.0, -2.0, 0.0)),
                0.0
            );

            // side
            assert_relative_eq!(
                capsule.signed_distance_to_point(Point3::new(1.0, 0.0, 0.0)),
                0.0
            );
        }
    }

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

        let container = ShapeContainer::default();
        let capsule = Shape::Capsule(Capsule::new(1.0, 1.0)).into_shape_ref(&container);

        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 0.0, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 0.5, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, -0.5, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 1.0, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, -1.0, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 1.5, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, -1.5, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 2.0, 0.0)),
            ContainsResult::Surface
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, -2.0, 0.0)),
            ContainsResult::Surface
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 2.1, 0.0)),
            ContainsResult::Outside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, -2.1, 0.0)),
            ContainsResult::Outside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 2.0, 0.1)),
            ContainsResult::Outside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, -2.0, 0.1)),
            ContainsResult::Outside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 2.0, -0.1)),
            ContainsResult::Outside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, -2.0, -0.1)),
            ContainsResult::Outside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, 2.1, 0.1)),
            ContainsResult::Outside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(0.0, -2.1, 0.1)),
            ContainsResult::Outside
        );
        assert_eq!(
            capsule.contains_point(Point3::new(1.0, 0.0, 0.0)),
            ContainsResult::Surface
        );
        assert_eq!(
            capsule.contains_point(Point3::new(-1.0, 0.0, 0.0)),
            ContainsResult::Surface
        );
        assert_eq!(
            capsule.contains_point(Point3::new(1.0, 0.1, 0.0)),
            ContainsResult::Surface
        );
        assert_eq!(
            capsule.contains_point(Point3::new(1.0, 0.0, 0.1)),
            ContainsResult::Outside
        );
    }

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

        let capsule = Capsule::new(1.0, 1.0);
        let (max_radius, max_angular_expansion) = capsule.max_radius_and_max_angular_expansion();
        assert_relative_eq!(max_radius, 2.0f32);
        assert_relative_eq!(max_angular_expansion, 1.0f32);
    }
}