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::{bool32x4, f32x4, u32x4};
use glam_det::{
    Isometry3, Isometry3x4, Mat3, Mat3x4, Point3, Point3x4, UnitQuat, UnitQuatx4, UnitVec3, Vec3,
    Vec3x4,
};
pub use phys_geom::{Aabb3, ComputeAabb3};

use crate::collision_tasks::ReductionId;
use crate::convex_contact_manifold::{self, Convex4ContactManifoldWide};
use crate::ShapeContainer;

#[derive(Copy, Clone, PartialEq, Eq, Debug)]
pub enum ContainsResult {
    Outside,
    Inside,
    Surface,
}

/// Determine whether the given point is inside, outside or on the surface of the shape.
/// # Examples
/// ```
/// use glam_det::Point3;
/// use phys_collision::{ContainsPoint, ContainsResult, Sphere};
/// let sphere = Sphere::new(1.0);
/// assert_eq!(sphere.contains_point(Point3::new(0.0, 0.0, 0.0)), ContainsResult::Inside);
/// ```
pub trait ContainsPoint {
    #[inline]
    fn contains_point(&self, local_point: Point3) -> ContainsResult {
        self.contains_point_with_threshold(local_point, f32::EPSILON)
    }

    fn contains_point_with_threshold(&self, local_point: Point3, threshold: f32) -> ContainsResult;
}

/// Computes the signed distance from given point to a shape. If the point is outside the shape, the
/// result is positive, otherwise negative.
/// # Examples
/// ```
/// use glam_det::Point3;
/// use phys_collision::{SignedDistanceToPoint, Sphere};
/// let sphere = Sphere::new(1.0);
/// assert_eq!(sphere.signed_distance_to_point(Point3::new(2.0, 0.0, 0.0)), 1.0);
/// ```
pub trait SignedDistanceToPoint {
    fn signed_distance_to_point(&self, local_point: Point3) -> f32;
}

pub trait ConvexShape {}

pub trait BaseShapeWide {
    type TShape: Default + Clone + Copy;
}

pub trait CreateShapeWide<const N: usize>: BaseShapeWide {
    fn create<'a>(iter: impl Iterator<Item = &'a Self::TShape> + Clone) -> Self
    where
        Self::TShape: 'a;
}

pub trait Expansion {
    // max_radius is the max rotation radius of this shape
    // max_angular_expansion = max_radius - min_radius
    // https://xxx.feishu.cn/wiki/IVjxwWJA9iCs19k0msvc4p5Anih
    fn max_radius_and_max_angular_expansion(&self) -> (f32, f32);
}

pub struct ContactContext<'a> {
    pub speculative_margin: f32x4,
    pub offset_b: &'a Vec3x4,
    pub orientation_a: &'a UnitQuatx4,
    pub orientation_b: &'a UnitQuatx4,
    pub pair_count: usize,
    pub complex_shape_container: Option<&'a ShapeContainer>,
}

// todo remove complex_shape_container from here,issue #1301
pub trait PairWideTest<TShapeWideA, TShapeWideB> {
    fn test(
        a: &TShapeWideA,
        b: &TShapeWideB,
        context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    );

    /// Indicates whether the manifold should be reset before testing.
    ///
    /// If the test will overwrite all the values of the manifold,
    /// the `should_reset_manifold_before_test` should return false.
    /// Conversely, if the test will only overwrite some of the values of the manifold,
    /// the `should_reset_manifold_before_test` should return true.
    fn should_reset_manifold_before_test() -> bool;
}

pub trait PairTest<TShapeA, TShapeB, TManifoldType> {
    fn test(
        a: TShapeA,
        b: TShapeB,
        speculative_margin: f32,
        offset_b: Vec3,
        orientation_a: UnitQuat,
        orientation_b: UnitQuat,
        complex_shape_container: Option<&ShapeContainer>,
    ) -> TManifoldType;
}

pub trait CollisionCallBack {
    //fn on_pair_complete(&mut self, pair_id: usize, manifold: &impl ContactManifold);
    fn on_pair_complete(&mut self, pair_id: usize, manifold: convex_contact_manifold::ManifoldRef);
}

pub trait CollisionPairWide {
    type TShapeWideA;
    type TShapeWideB;
    fn flip_mask(&self) -> &bool32x4;
    fn speculative_margin(&self) -> &f32x4;
    fn shape_a(&self) -> &Self::TShapeWideA;
    fn shape_b(&self) -> &Self::TShapeWideB;
    fn orientation_a(&self) -> &UnitQuatx4;
    fn orientation_b(&self) -> &UnitQuatx4;
    fn offset_b(&self) -> &Vec3x4;
    fn set_offset_b(&mut self, value: Vec3x4);

    fn reduction_id(&self) -> &[ReductionId; 4];
}

pub trait PairWide {
    type TPair;
}

pub trait WritePairWide<const N: usize>: PairWide {
    fn write_wide(&mut self, array_slice: [&Self::TPair; N]);
}

/// Describes structures that have one or more contacts, which may be manifolds formed by convex
/// shapes, or formed by non-convex shapes.
pub trait ContactManifold {
    const MAX_CONTACTS: usize;

    fn offset_b(&self) -> Vec3;
    fn contact_count(&self) -> usize;
    fn get_contact_normal(&self, contact_index: usize) -> UnitVec3;
    fn get_contact_offset_a(&self, contact_index: usize) -> Vec3;
    fn get_contact_depth(&self, contact_index: usize) -> f32;
    fn get_contact_feature_id(&self, contact_index: usize) -> u32;

    #[cfg(test)]
    fn is_convex(&self) -> bool;
}

pub trait ContactManifoldWide<TManifold: ContactManifold> {
    fn apply_flip_mask(&mut self, count: usize, offset_b: &mut Vec3x4, flip_mask: &bool32x4);
    ///The test `collision_tasks::benches::basic_wide::test_basic_wide_pack_unpack` indicates that
    /// changing from returning a value of `TManifold` to passing a reference of `TManifold` can
    /// lead to a 50% performance improvement.
    fn get_manifold(&self, index: usize, count: usize, offset_b: &Vec3x4, result: &mut TManifold);
}

pub trait PairBatch<Type> {
    fn push(&mut self, pair: Type);

    fn get_pairs(&self) -> &[Type];
    fn new(size: usize) -> Self;
    fn clear(&mut self);
}

#[derive(Default, Clone, Copy)]
pub struct MinkowskiSupportResult {
    pub point: Point3,
    pub point_index: usize,
}

/// minkowski support function for primitive
pub trait MinkowskiSupport {
    fn support_point(&self, direction: Vec3, transform: &Isometry3) -> MinkowskiSupportResult;
}

pub struct MinkowskiSupportResultWide {
    pub point: Point3x4,
    #[allow(dead_code)]
    pub point_index: u32x4,
}

pub trait OrientationWide {
    fn mul_vec3(&self, value: Vec3x4) -> Vec3x4;

    fn inverse_mul_vec3(&self, value: Vec3x4) -> Vec3x4;
}

impl OrientationWide for UnitQuatx4 {
    #[inline]
    fn mul_vec3(&self, value: Vec3x4) -> Vec3x4 {
        UnitQuatx4::mul_vec3(*self, value)
    }

    #[inline]
    fn inverse_mul_vec3(&self, value: Vec3x4) -> Vec3x4 {
        self.inverse().mul_vec3(value)
    }
}

impl OrientationWide for Mat3x4 {
    #[inline]
    fn mul_vec3(&self, value: Vec3x4) -> Vec3x4 {
        self * value
    }

    #[inline]
    fn inverse_mul_vec3(&self, value: Vec3x4) -> Vec3x4 {
        self.transpose() * value
    }
}

impl OrientationWide for Isometry3x4 {
    #[inline]
    fn mul_vec3(&self, value: Vec3x4) -> Vec3x4 {
        self.transform_vector3(value)
    }

    #[inline]
    fn inverse_mul_vec3(&self, value: Vec3x4) -> Vec3x4 {
        self.inverse().transform_vector3(value)
    }
}

pub(crate) trait Orientation {
    fn mul_vec3(&self, value: Vec3) -> Vec3;

    fn inverse_mul_vec3(&self, value: Vec3) -> Vec3;
}

impl Orientation for UnitQuat {
    #[inline]
    fn mul_vec3(&self, value: Vec3) -> Vec3 {
        UnitQuat::mul_vec3(*self, value)
    }

    #[inline]
    fn inverse_mul_vec3(&self, value: Vec3) -> Vec3 {
        self.inverse().mul_vec3(value)
    }
}

impl Orientation for Mat3 {
    #[inline]
    fn mul_vec3(&self, value: Vec3) -> Vec3 {
        self * value
    }

    #[inline]
    fn inverse_mul_vec3(&self, value: Vec3) -> Vec3 {
        self.transpose() * value
    }
}

impl Orientation for Isometry3 {
    #[inline]
    fn mul_vec3(&self, value: Vec3) -> Vec3 {
        self.transform_vector3(value)
    }

    #[inline]
    fn inverse_mul_vec3(&self, value: Vec3) -> Vec3 {
        self.inverse().transform_vector3(value)
    }
}

pub trait MinkowskiSupportWide {
    #[inline]
    fn support_point(
        &self,
        direction: Vec3x4,
        orientation: &impl OrientationWide,
        container: Option<&ShapeContainer>,
    ) -> MinkowskiSupportResultWide {
        let local_direction = orientation.inverse_mul_vec3(direction);
        let mut result = self.support_point_local(local_direction, container);
        result.point = Point3x4::from_vec3x4(orientation.mul_vec3(result.point.as_vec3x4()));
        result
    }
    fn support_point_local(
        &self,
        local_direction: Vec3x4,
        container: Option<&ShapeContainer>,
    ) -> MinkowskiSupportResultWide;
}

pub struct ArrayGetter<const N: usize> {}

impl<const N: usize> ArrayGetter<N> {
    #[inline]
    #[must_use]
    pub fn get_vec3x4_from_array4(array: [Vec3; 4]) -> Vec3x4 {
        Vec3x4::from([
            f32x4::from([array[0].x, array[1].x, array[2].x, array[3].x]),
            f32x4::from([array[0].y, array[1].y, array[2].y, array[3].y]),
            f32x4::from([array[0].z, array[1].z, array[2].z, array[3].z]),
        ])
    }

    #[must_use]
    pub fn get_point3x4_from_array4(array: [Point3; 4]) -> Point3x4 {
        Point3x4::from([
            f32x4::from([array[0].x, array[1].x, array[2].x, array[3].x]),
            f32x4::from([array[0].y, array[1].y, array[2].y, array[3].y]),
            f32x4::from([array[0].z, array[1].z, array[2].z, array[3].z]),
        ])
    }
}

impl ArrayGetter<4> {
    pub fn get_array4_from_iter<T>(mut iter: impl Iterator<Item = T>, _: T) -> [T; 4]
    where
        T: Copy,
    {
        [
            iter.next().unwrap(),
            iter.next().unwrap(),
            iter.next().unwrap(),
            iter.next().unwrap(),
        ]
    }

    pub fn get_vec3x4_from_iter(mut iter: impl Iterator<Item = Vec3>) -> Vec3x4 {
        let item0 = iter.next().unwrap();
        let item1 = iter.next().unwrap();
        let item2 = iter.next().unwrap();
        let item3 = iter.next().unwrap();
        Vec3x4::from([
            f32x4::from([item0.x, item1.x, item2.x, item3.x]),
            f32x4::from([item0.y, item1.y, item2.y, item3.y]),
            f32x4::from([item0.z, item1.z, item2.z, item3.z]),
        ])
    }

    pub fn get_unit_quat3x4_from_iter(
        mut iter: impl Iterator<Item = UnitQuat> + Clone,
    ) -> UnitQuatx4 {
        let item0 = iter.next().unwrap();
        let item1 = iter.next().unwrap();
        let item2 = iter.next().unwrap();
        let item3 = iter.next().unwrap();
        UnitQuatx4::from_xyzw_unchecked(
            f32x4::from([item0.x, item1.x, item2.x, item3.x]),
            f32x4::from([item0.y, item1.y, item2.y, item3.y]),
            f32x4::from([item0.z, item1.z, item2.z, item3.z]),
            f32x4::from([item0.w, item1.w, item2.w, item3.w]),
        )
    }
}

impl ArrayGetter<3> {
    pub fn get_array4_from_iter<T>(mut iter: impl Iterator<Item = T>, default: T) -> [T; 4]
    where
        T: Copy,
    {
        [
            iter.next().unwrap(),
            iter.next().unwrap(),
            iter.next().unwrap(),
            default,
        ]
    }

    pub fn get_vec3x4_from_iter(mut iter: impl Iterator<Item = Vec3>) -> Vec3x4 {
        let item0 = iter.next().unwrap();
        let item1 = iter.next().unwrap();
        let item2 = iter.next().unwrap();
        let item3 = Vec3::default();
        Vec3x4::from([
            f32x4::from([item0.x, item1.x, item2.x, item3.x]),
            f32x4::from([item0.y, item1.y, item2.y, item3.y]),
            f32x4::from([item0.z, item1.z, item2.z, item3.z]),
        ])
    }

    pub fn get_unit_quat3x4_from_iter(
        mut iter: impl Iterator<Item = UnitQuat> + Clone,
    ) -> UnitQuatx4 {
        let item0 = iter.next().unwrap();
        let item1 = iter.next().unwrap();
        let item2 = iter.next().unwrap();
        let item3 = UnitQuat::IDENTITY;
        UnitQuatx4::from_xyzw_unchecked(
            f32x4::from([item0.x, item1.x, item2.x, item3.x]),
            f32x4::from([item0.y, item1.y, item2.y, item3.y]),
            f32x4::from([item0.z, item1.z, item2.z, item3.z]),
            f32x4::from([item0.w, item1.w, item2.w, item3.w]),
        )
    }
}

impl ArrayGetter<2> {
    pub fn get_array4_from_iter<T>(mut iter: impl Iterator<Item = T>, default: T) -> [T; 4]
    where
        T: Copy,
    {
        [iter.next().unwrap(), iter.next().unwrap(), default, default]
    }

    pub fn get_vec3x4_from_iter(mut iter: impl Iterator<Item = Vec3>) -> Vec3x4 {
        let item0 = iter.next().unwrap();
        let item1 = iter.next().unwrap();
        let item2 = Vec3::default();
        let item3 = Vec3::default();
        Vec3x4::from([
            f32x4::from([item0.x, item1.x, item2.x, item3.x]),
            f32x4::from([item0.y, item1.y, item2.y, item3.y]),
            f32x4::from([item0.z, item1.z, item2.z, item3.z]),
        ])
    }

    pub fn get_unit_quat3x4_from_iter(
        mut iter: impl Iterator<Item = UnitQuat> + Clone,
    ) -> UnitQuatx4 {
        let item0 = iter.next().unwrap();
        let item1 = iter.next().unwrap();
        let item2 = UnitQuat::IDENTITY;
        let item3 = UnitQuat::IDENTITY;
        UnitQuatx4::from_xyzw_unchecked(
            f32x4::from([item0.x, item1.x, item2.x, item3.x]),
            f32x4::from([item0.y, item1.y, item2.y, item3.y]),
            f32x4::from([item0.z, item1.z, item2.z, item3.z]),
            f32x4::from([item0.w, item1.w, item2.w, item3.w]),
        )
    }
}

impl ArrayGetter<1> {
    pub fn get_array4_from_iter<T>(mut iter: impl Iterator<Item = T>, default: T) -> [T; 4]
    where
        T: Copy,
    {
        [iter.next().unwrap(), default, default, default]
    }

    pub fn get_vec3x4_from_iter(mut iter: impl Iterator<Item = Vec3>) -> Vec3x4 {
        let item0 = iter.next().unwrap();
        let item1 = Vec3::default();
        let item2 = Vec3::default();
        let item3 = Vec3::default();
        Vec3x4::from([
            f32x4::from([item0.x, item1.x, item2.x, item3.x]),
            f32x4::from([item0.y, item1.y, item2.y, item3.y]),
            f32x4::from([item0.z, item1.z, item2.z, item3.z]),
        ])
    }

    pub fn get_unit_quat3x4_from_iter(
        mut iter: impl Iterator<Item = UnitQuat> + Clone,
    ) -> UnitQuatx4 {
        let item0 = iter.next().unwrap();
        let item1 = UnitQuat::IDENTITY;
        let item2 = UnitQuat::IDENTITY;
        let item3 = UnitQuat::IDENTITY;
        UnitQuatx4::from_xyzw_unchecked(
            f32x4::from([item0.x, item1.x, item2.x, item3.x]),
            f32x4::from([item0.y, item1.y, item2.y, item3.y]),
            f32x4::from([item0.z, item1.z, item2.z, item3.z]),
            f32x4::from([item0.w, item1.w, item2.w, item3.w]),
        )
    }
}