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

use crate::traits::*;
use crate::ShapeContainer;

impl ContainsPoint for Sphere {
    fn contains_point_with_threshold(&self, local_point: Point3, threshold: f32) -> ContainsResult {
        let radius = self.radius();
        let distance_squared = local_point.as_vec3().length_squared();
        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 Sphere {
    fn signed_distance_to_point(&self, local_point: Point3) -> f32 {
        let distance_squared = local_point.as_vec3().length_squared();

        let distance = distance_squared.sqrtf();

        distance - self.radius()
    }
}

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

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

impl MinkowskiSupportWide for SphereWide {
    fn support_point_local(
        &self,
        local_direction: Vec3x4,
        _: Option<&ShapeContainer>,
    ) -> MinkowskiSupportResultWide {
        let result = Point3x4::from_vec3x4(local_direction.normalize() * self.radius);
        MinkowskiSupportResultWide {
            point: result,
            point_index: u32x4::ZERO,
        }
    }
}

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

impl BaseShapeWide for SphereWide {
    type TShape = Sphere;
}

macro_rules! impl_sphere_wide {
    ($($num:tt),*) => {
        $(
           impl CreateShapeWide<$num> for SphereWide {
                fn create<'a>(iter: impl Iterator<Item=&'a Self::TShape>) -> Self where Self::TShape: 'a {
                    Self {
                        radius: f32x4::from(ArrayGetter::<$num>::get_array4_from_iter(iter.map( #[inline]|x| x.radius()), 0.0)),
                    }
                }
           }
        )*
    };
}

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

impl MinkowskiSupport for Sphere {
    fn support_point(&self, direction: Vec3, transform: &Isometry3) -> MinkowskiSupportResult {
        let direction = transform.inverse().transform_vector3(direction);
        let result =
            transform.transform_point3(Point3::from_vec3(direction.normalize() * self.radius()));
        MinkowskiSupportResult {
            point: result,
            point_index: 0,
        }
    }
}

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

            let container = ShapeContainer::default();
            let sphere = Shape::Sphere(Sphere::new(1.0)).into_shape_ref(&container);
            assert_relative_eq!(
                sphere.signed_distance_to_point(Point3::new(0.0, 0.0, 0.0)),
                -1.0
            );

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

            assert_relative_eq!(
                sphere.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 sphere = Shape::Sphere(Sphere::new(1.0)).into_shape_ref(&container);
        assert_eq!(
            sphere.contains_point(Point3::new(0.0, 0.0, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            sphere.contains_point(Point3::new(0.0, 0.5, 0.0)),
            ContainsResult::Inside
        );
        assert_eq!(
            sphere.contains_point(Point3::new(0.0, 1.0, 0.0)),
            ContainsResult::Surface
        );
        assert_eq!(
            sphere.contains_point(Point3::new(0.0, 1.1, 0.0)),
            ContainsResult::Outside
        );
    }

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

        let sphere = Sphere::new(1.0);
        let (max_radius, max_angular_expansion) = sphere.max_radius_and_max_angular_expansion();
        assert_relative_eq!(max_radius, 1.0);
        assert_relative_eq!(max_angular_expansion, 0.0);
    }
}