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::Cylinder;

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

impl MinkowskiSupport for Cylinder {
    #[inline]
    fn support_point(&self, direction: Vec3, transform: &Isometry3) -> MinkowskiSupportResult {
        let half_height = self.half_height();
        let radius = self.radius();
        let direction = transform.inverse().transform_vector3(direction);
        let result_y = if direction.y > 0.0f32 {
            half_height
        } else {
            -half_height
        };
        let xz_length_squared = direction.x * direction.x + direction.z * direction.z;
        let result = if xz_length_squared > 1e-8 {
            let xz_length = xz_length_squared.sqrtf();
            let result_x = direction.x / xz_length * radius;
            let result_z = direction.z / xz_length * radius;
            Point3::new(result_x, result_y, result_z)
        } else {
            // parallel to y axis (cylinder side)
            Point3::new(0.0, result_y, 0.0)
        };
        MinkowskiSupportResult {
            point: transform.transform_point3(result),
            point_index: 0,
        }
    }
}

impl ContainsPoint for Cylinder {
    fn contains_point_with_threshold(&self, local_point: Point3, threshold: f32) -> ContainsResult {
        let half_height = self.half_height();
        let radius = self.radius();
        let local_point = local_point.as_vec3();
        let distance_y = local_point.y.absf() - half_height;
        let mut is_on_surface_y = false;

        if distance_y.absf() < threshold {
            is_on_surface_y = true;
        } else if distance_y > 0.0 {
            return ContainsResult::Outside;
        }

        let distance_xz =
            (local_point.x * local_point.x + local_point.z * local_point.z) - radius * radius;

        if distance_xz.absf() < threshold {
            ContainsResult::Surface
        } else if distance_xz < 0.0 {
            if is_on_surface_y {
                ContainsResult::Surface
            } else {
                ContainsResult::Inside
            }
        } else {
            ContainsResult::Outside
        }
    }
}

impl SignedDistanceToPoint for Cylinder {
    fn signed_distance_to_point(&self, local_point: Point3) -> f32 {
        // TODO: There may be a better way. (issue #1435)

        let abs_point_y = local_point.y.absf();
        let squared_distance_xz = local_point.x * local_point.x + local_point.z * local_point.z;

        if abs_point_y < self.half_height() {
            let distance_to_side = squared_distance_xz.sqrtf() - self.radius();
            if distance_to_side < 0.0 {
                distance_to_side.max(abs_point_y - self.half_height())
            } else {
                distance_to_side
            }
        } else if squared_distance_xz < self.radius() * self.radius() {
            abs_point_y - self.half_height()
        } else {
            let distance_xz = squared_distance_xz.sqrtf() - self.radius();
            let distance_y = abs_point_y - self.half_height();
            (distance_xz * distance_xz + distance_y * distance_y).sqrtf()
        }
    }
}

impl ConvexShape for Cylinder {}

impl Expansion for Cylinder {
    #[inline]
    fn max_radius_and_max_angular_expansion(&self) -> (f32, f32) {
        let half_height = self.half_height();
        let radius = self.radius();
        let mut max_radius = half_height * half_height + radius * radius;
        max_radius = max_radius.sqrtf();
        (max_radius, max_radius - radius.min(half_height))
    }
}

/// Cylinder shape wide support
pub struct CylinderWide {
    pub half_height: f32x4,
    pub radius: f32x4,
}

impl Default for CylinderWide {
    #[inline]
    fn default() -> Self {
        Self {
            half_height: f32x4::ZERO,
            radius: f32x4::ZERO,
        }
    }
}

macro_rules! impl_cylinder_wide {
    ($($num:tt),*) => {
        $(
          impl CreateShapeWide<$num> for CylinderWide {
            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]|cylinder| {
                        cylinder.half_height()
                    }), 0.0)),
                    radius: f32x4::from(ArrayGetter::<$num>::get_array4_from_iter(iter.clone().map(#[inline]|cylinder| {
                        cylinder.radius()
                    }), 0.0)),
                }
            }
          }
        )*
    }

}

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

impl BaseShapeWide for CylinderWide {
    type TShape = Cylinder;
}

impl MinkowskiSupportWide for CylinderWide {
    fn support_point_local(
        &self,
        local_direction: Vec3x4,
        _: Option<&ShapeContainer>,
    ) -> MinkowskiSupportResultWide {
        let support_y = self
            .half_height
            .select(local_direction.y.gt(f32x4::ZERO), -self.half_height);
        let xz_length =
            (local_direction.x * local_direction.x + local_direction.z * local_direction.z).sqrtf();
        let xz_normalized = self.radius / xz_length;
        let not_parallel_to_cylinder_side = xz_length.gt(f32x4::splat(1e-8));
        let support_x =
            (local_direction.x * xz_normalized).select(not_parallel_to_cylinder_side, f32x4::ZERO);
        let support_z =
            (local_direction.z * xz_normalized).select(not_parallel_to_cylinder_side, f32x4::ZERO);
        MinkowskiSupportResultWide {
            point: Point3x4::new(support_x, support_y, support_z),
            point_index: u32x4::ZERO,
        }
    }
}

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

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

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

        let cylinder = Cylinder::new(2.0, 3.0);
        assert!((cylinder.half_height() - 2.0).absf() < f32::EPSILON);
        assert!((cylinder.radius() - 3.0).absf() < f32::EPSILON);
        assert!((cylinder.height() - 4.0).absf() < f32::EPSILON);
        assert!((cylinder.compute_volume() - std::f32::consts::PI * 36.0).absf() < f32::EPSILON);
    }

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

        let shape = Cylinder::new(2f32, 1f32);
        let (max_radius, max_angular_expansion) = shape.max_radius_and_max_angular_expansion();
        assert_relative_eq!(max_radius, 5f32.sqrt());
        assert_relative_eq!(max_angular_expansion, 5f32.sqrt() - 1f32);
    }

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

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

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

            macro_rules! test_both {
                ($point:expr, $res:expr, $distance:expr) => {
                    assert_eq!(cylinder.contains_point($point), $res);
                    assert_relative_eq!(cylinder.signed_distance_to_point($point), $distance);
                };
            }

            // infinite cylinder
            test_both!(Point3::new(1.0, 0.0, 0.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(-1.0, 0.0, 0.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(0.0, 0.0, 1.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(0.0, 0.0, -1.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(1.0, 1.0, 0.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(1.0, 2.0, 0.0), ContainsResult::Outside, 1.0);
            test_both!(Point3::new(2.0, 0.0, 0.0), ContainsResult::Outside, 1.0);

            // top cap
            test_both!(Point3::new(0.0, 1.0, 0.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(0.5, 1.0, 0.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(1.1, 1.0, 0.0), ContainsResult::Outside, 0.1);
            test_both!(Point3::new(1.0, 1.1, 0.0), ContainsResult::Outside, 0.1);
            test_both!(
                Point3::new(1.1, 1.1, 0.0),
                ContainsResult::Outside,
                (0.1 * 0.1 + 0.1 * 0.1).sqrtf()
            );

            // bottom cap
            test_both!(Point3::new(0.0, -1.0, 0.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(0.5, -1.0, 0.0), ContainsResult::Surface, 0.0);
            test_both!(Point3::new(1.1, -1.0, 0.0), ContainsResult::Outside, 0.1);
            test_both!(Point3::new(1.0, -1.1, 0.0), ContainsResult::Outside, 0.1);
            test_both!(
                Point3::new(1.1, -1.1, 0.0),
                ContainsResult::Outside,
                (0.1 * 0.1 + 0.1 * 0.1).sqrtf()
            );

            // inside
            test_both!(Point3::new(0.5, 0.8, 0.0), ContainsResult::Inside, -0.2);
            test_both!(Point3::new(0.5, -0.8, 0.0), ContainsResult::Inside, -0.2);
            test_both!(Point3::new(0.5, 0.0, 0.0), ContainsResult::Inside, -0.5);
        }
    }

    #[cfg(test)]
    mod minkowski_support_cylinder_tests {
        use glam_det::{Isometry3x4, Mat3x4, UnitQuatx4};
        use wasm_bindgen_test::*;

        use super::*;

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

            let cylinder = Cylinder::new(1.0, 2.0);
            let direction = Vec3::new(1.0, 1.0, 1.0);
            let transform = Isometry3::IDENTITY;
            let support_point = cylinder.support_point(direction, &transform);
            assert_eq!(
                support_point.point,
                Point3::new(2_f32.sqrt(), 1.0, 2_f32.sqrt())
            );
            assert_eq!(support_point.point_index, 0);

            let direction = Vec3::new(-3.0, -0.1, -4.0);
            let support_point = cylinder.support_point(direction, &transform);
            assert_eq!(support_point.point, Point3::new(-1.2, -1.0, -1.6));
            assert_eq!(support_point.point_index, 0);

            let direction = Vec3::new(0.0, -0.1, 0.0);
            let support_point = cylinder.support_point(direction, &transform);
            assert_eq!(support_point.point, Point3::new(0.0, -1.0, 0.0));
            assert_eq!(support_point.point_index, 0);
        }

        const EPSILON: f32 = 1e-6;

        fn check_minkowski_result_wide(
            result: &MinkowskiSupportResultWide,
            correct_answer: &MinkowskiSupportResultWide,
        ) {
            assert!(
                (result.point.as_vec3x4() - correct_answer.point.as_vec3x4())
                    .cmplt(Vec3x4::splat(f32x4::splat(EPSILON)))
                    .all()
                    .all()
            );
            assert_eq!(result.point_index, correct_answer.point_index);
        }

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

            let cylinder_1 = Cylinder::new(1.0, 2.0);
            let cylinder_2 = Cylinder::new(3.0, 4.0);
            let cylinder_3 = Cylinder::new(5.0, 6.0);
            let cylinder_4 = Cylinder::new(7.0, 8.0);
            let cylinder_wide = <CylinderWide as CreateShapeWide<4>>::create(
                [cylinder_1, cylinder_2, cylinder_3, cylinder_4].iter(),
            );
            assert_eq!(cylinder_wide.radius, f32x4::from([2.0, 4.0, 6.0, 8.0]));
            assert_eq!(cylinder_wide.half_height, f32x4::from([1.0, 3.0, 5.0, 7.0]));

            let direction_wide = Vec3x4::new(
                f32x4::from([1.0, -3.0, 0.0, 1.0]),
                f32x4::from([1.0, -0.1, -0.1, 1.0]),
                f32x4::from([1.0, -4.0, 0.0, 1.0]),
            );
            // rotation is identity, use local
            let support_point_wide = cylinder_wide.support_point_local(direction_wide, None);

            let correct_result = MinkowskiSupportResultWide {
                point: Point3x4::new(
                    f32x4::from([
                        std::f32::consts::SQRT_2,
                        -2.4,
                        0.0,
                        4.0 * std::f32::consts::SQRT_2,
                    ]),
                    f32x4::from([1.0, -3.0, -5.0, 7.0]),
                    f32x4::from([
                        std::f32::consts::SQRT_2,
                        -3.2,
                        0.0,
                        4.0 * std::f32::consts::SQRT_2,
                    ]),
                ),
                point_index: u32x4::ZERO,
            };
            check_minkowski_result_wide(&support_point_wide, &correct_result);

            // if rotation is not identity
            let quat_wide = UnitQuatx4 {
                x: f32x4::from([1.0, 0.0, -1.0, 1.0]),
                y: f32x4::from([2.0, 0.0, -2.0, 2.0]),
                z: f32x4::from([3.0, 1.0, 3.0, -3.0]),
                w: f32x4::from([1.0, 1.0, 1.0, 1.0]),
            }
            .renormalize();
            let orientation = Mat3x4::from_quat(quat_wide);
            let support_point_wide = cylinder_wide.support_point(direction_wide, &quat_wide, None);

            let correct_result = MinkowskiSupportResultWide {
                point: Point3x4::new(
                    f32x4::from([
                        1.140_314_8_f32,
                        -2.999_999_8_f32,
                        -0.383_824_1_f32,
                        1.086_073_3_f32,
                    ]),
                    f32x4::from([
                        1.052_225_4_f32,
                        -0.099_968_7_f32,
                        -7.323_521_f32,
                        6.517_144_f32,
                    ]),
                    f32x4::from([
                        1.610_125_5_f32,
                        -3.998_750_7_f32,
                        -2.686_767_f32,
                        8.327_5_f32,
                    ]),
                ),
                point_index: u32x4::ZERO,
            };
            check_minkowski_result_wide(&support_point_wide, &correct_result);

            let support_point_wide =
                cylinder_wide.support_point(direction_wide, &orientation, None);
            check_minkowski_result_wide(&support_point_wide, &correct_result);

            let support_point_wide =
                cylinder_wide.support_point(direction_wide, &orientation, None);
            check_minkowski_result_wide(&support_point_wide, &correct_result);

            let orientation = Isometry3x4::from_quat(quat_wide);
            let support_point_wide =
                cylinder_wide.support_point(direction_wide, &orientation, None);
            check_minkowski_result_wide(&support_point_wide, &correct_result);
        }
    }
}