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::{bool32x4, f32x4, u32x4};
use glam_det::{Isometry3, Point3, UnitVec3, UnitVec3x4, Vec3, Vec3x4};

use crate::collision_tasks::ConcaveManifold;
use crate::traits::{ContactManifold, ContactManifoldWide};

pub enum ManifoldRef<'a> {
    Convex(&'a ConvexContactManifold),
    Concave(&'a ConcaveManifold),
}
#[derive(Default)]
pub struct ConvexContactManifold {
    /// The body B's center of mass offset from the body A's center of mass in world space.
    pub offset_b: Vec3,
    /// The normal of the contact manifold. Direction is from body B to body A.
    pub normal: UnitVec3,
    /// The number of contacts in the manifold.
    pub count: usize,
    /// The contact points of the manifold.
    pub contacts: [ConvexContact; 4],
}

impl ContactManifold for ConvexContactManifold {
    const MAX_CONTACTS: usize = 4;

    fn offset_b(&self) -> Vec3 {
        self.offset_b
    }

    #[inline]
    fn contact_count(&self) -> usize {
        self.count
    }

    #[inline]
    fn get_contact_normal(&self, _: usize) -> UnitVec3 {
        self.normal
    }

    #[inline]
    fn get_contact_offset_a(&self, contact_index: usize) -> Vec3 {
        self.contacts[contact_index].offset_a
    }

    #[inline]
    fn get_contact_depth(&self, contact_index: usize) -> f32 {
        self.contacts[contact_index].depth
    }

    #[inline]
    fn get_contact_feature_id(&self, contact_index: usize) -> u32 {
        self.contacts[contact_index].feature_id
    }

    #[cfg(test)]
    #[inline]
    fn is_convex(&self) -> bool {
        true
    }
}

#[derive(Clone)]
pub struct ConvexContact {
    /// The contact point offset from the body A's center of mass in world space.
    ///
    /// # Note
    ///
    /// The contact point is always on the surface of body A.
    pub offset_a: Vec3,
    /// The penetration depth. Always positive.
    pub depth: f32,
    pub feature_id: u32,
}

impl Default for ConvexContact {
    #[inline]
    fn default() -> Self {
        Self {
            offset_a: Vec3::default(),
            depth: 0.0,
            feature_id: 0,
        }
    }
}

#[derive(PartialEq, Debug)]
pub struct Convex4ContactManifoldWide {
    pub offset_a: [Vec3x4; 4],
    pub normal: UnitVec3x4,
    pub depth: [f32x4; 4],
    pub feature_id: [u32x4; 4],
    pub contact_exists: [bool32x4; 4],
}

#[allow(dead_code)]
#[derive(PartialEq, Debug)]
pub struct Convex4ContactManifold {
    pub offset_a: [Vec3; 4],
    pub normal: UnitVec3,
    pub depth: [f32; 4],
    pub feature_id: [u32; 4],
    pub contact_exists: [bool; 4],
}

impl ContactManifoldWide<ConvexContactManifold> for Convex4ContactManifoldWide {
    #[inline]
    fn apply_flip_mask(&mut self, count: usize, offset_b: &mut Vec3x4, flip_mask: &bool32x4) {
        let flipped_offset = -*offset_b;
        let flipped_normal = -self.normal;
        let mut flipped_offset_a = [Vec3x4::ZERO; 4];
        for (flipped_offset_a, (offset_a, depth)) in flipped_offset_a
            .iter_mut()
            .zip(self.offset_a.iter().zip(self.depth.iter()))
            .take(count)
        {
            *flipped_offset_a = offset_a + self.normal * depth - *offset_b;
        }
        self.normal = UnitVec3x4::lane_select(*flip_mask, flipped_normal, self.normal);
        *offset_b = Vec3x4::lane_select(*flip_mask, flipped_offset, *offset_b);
        for (&flipped_offset_a, offset_a) in flipped_offset_a
            .iter()
            .zip(self.offset_a.iter_mut())
            .take(count)
        {
            *offset_a = Vec3x4::lane_select(*flip_mask, flipped_offset_a, *offset_a);
        }
    }

    #[inline]
    fn get_manifold(
        &self,
        index: usize,
        count: usize,
        offset_wide: &Vec3x4,
        result: &mut ConvexContactManifold,
    ) {
        result.count = 0;
        for i in 0..count {
            if self.contact_exists[i].extract(index) {
                let output_contact = &mut result.contacts[result.count];
                output_contact.offset_a = self.offset_a[i].extract_lane(index);
                output_contact.depth = self.depth[i].extract(index);
                output_contact.feature_id = self.feature_id[i].extract(index);
                result.count += 1;
            }
        }
        if result.count > 0 {
            result.offset_b = offset_wide.extract_lane(index);
            result.normal = self.normal.extract_lane(index);
        }
    }
}

impl Default for Convex4ContactManifoldWide {
    #[inline]
    fn default() -> Self {
        Self {
            offset_a: Default::default(),
            normal: UnitVec3x4::default(),
            depth: [f32x4::ZERO; 4],
            feature_id: [u32x4::ZERO; 4],
            contact_exists: [bool32x4::FALSE; 4],
        }
    }
}

impl Convex4ContactManifoldWide {
    #[inline]
    pub fn reset(&mut self, count: usize) {
        for i in 0..count {
            self.contact_exists[i] = bool32x4::FALSE;
        }
    }

    #[inline]
    pub fn write_slot_with_candidate_scalar(
        &mut self,
        candidate: ManifoldCandidate,
        i: usize,
        contact_index: usize,
        face_b: &Face,
        transform_b: Isometry3,
    ) {
        let offset_a =
            face_b.origin + face_b.tangent_x * candidate.x + face_b.tangent_y * candidate.y;
        let offset_a = transform_b.transform_point3(offset_a);
        self.offset_a[contact_index].replace_lane(i, offset_a.as_vec3());
        self.depth[contact_index].replace(i, candidate.depth);
        self.feature_id[contact_index].replace(i, candidate.feature_id);
        self.contact_exists[contact_index].replace(i, true);
    }
}

#[derive(Copy, Clone, Debug)]
pub struct ManifoldCandidateWide {
    pub x: f32x4,
    pub y: f32x4,
    pub depth: f32x4,
    pub feature_id: u32x4,
}

#[derive(Copy, Clone, Debug, Default)]
pub struct ManifoldCandidate {
    pub x: f32,
    pub y: f32,
    pub feature_id: u32,
    pub depth: f32,
}

impl ManifoldCandidate {
    #[inline]
    #[must_use]
    pub fn new(x: f32, y: f32, feature_id: u32) -> Self {
        Self {
            x,
            y,
            feature_id,
            depth: 0.0,
        }
    }
}

pub struct Face {
    pub origin: Point3,
    pub tangent_x: UnitVec3,
    pub tangent_y: UnitVec3,
}

impl Default for ManifoldCandidateWide {
    #[inline]
    fn default() -> Self {
        Self {
            x: f32x4::ZERO,
            y: f32x4::ZERO,
            depth: f32x4::ZERO,
            feature_id: u32x4::ZERO,
        }
    }
}

/// Specifically used as manifold data generated by the height field for temporary storage
#[derive(Default, Clone)]
pub struct OneContact {
    /// The body B's center of mass offset from the body A's center of mass in world space.
    pub offset_b: Vec3,
    /// The normal of the contact manifold. Direction is from body B to body A.
    pub normal: UnitVec3,
    /// The contact points of the manifold from body A's center.
    pub offset_a: Vec3,
    /// The penetration depth. Always positive.
    pub depth: f32,
    pub feature_id: u32,
}

#[cfg(test)]
mod tests {
    use approx_det::{AbsDiffEq, RelativeEq};

    use crate::convex_contact_manifold::*;

    impl AbsDiffEq for Convex4ContactManifoldWide {
        type Epsilon = f32;

        fn default_epsilon() -> Self::Epsilon {
            f32::default_epsilon()
        }

        fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
            self.offset_a.iter().zip(other.offset_a.iter()).all(
                #[inline]
                |(a, b)| abs_diff_eq_vec3x4(a, b, epsilon),
            ) && abs_diff_eq_uint_vec3x4(&self.normal, &other.normal, epsilon)
                && self.depth.iter().zip(other.depth.iter()).all(
                    #[inline]
                    |(&a, b)| a.abs_diff_eq(b, epsilon),
                )
                && self.feature_id.iter().zip(other.feature_id.iter()).all(
                    #[inline]
                    |(a, b)| a.eq(b).all(),
                )
                && self
                    .contact_exists
                    .iter()
                    .zip(other.contact_exists.iter())
                    .all(
                        #[inline]
                        |(&a, b)| a.eq(b).all(),
                    )
        }
    }

    impl RelativeEq for Convex4ContactManifoldWide {
        fn default_max_relative() -> Self::Epsilon {
            f32::default_max_relative()
        }

        fn relative_eq(
            &self,
            other: &Self,
            epsilon: Self::Epsilon,
            max_relative: Self::Epsilon,
        ) -> bool {
            self.offset_a.iter().zip(other.offset_a.iter()).all(
                #[inline]
                |(a, b)| relative_eq_vec3x4(a, b, epsilon, max_relative),
            ) && relative_eq_uint_vec3x4(&self.normal, &other.normal, epsilon, max_relative)
                && self.depth.iter().zip(other.depth.iter()).all(
                    #[inline]
                    |(&a, b)| a.relative_eq(b, epsilon, max_relative).all(),
                )
                && self.feature_id.iter().zip(other.feature_id.iter()).all(
                    #[inline]
                    |(a, b)| a.eq(b).all(),
                )
                && self
                    .contact_exists
                    .iter()
                    .zip(other.contact_exists.iter())
                    .all(
                        #[inline]
                        |(a, b)| a.eq(b).all(),
                    )
        }
    }

    fn relative_eq_vec3x4(left: &Vec3x4, right: &Vec3x4, epsilon: f32, max_relative: f32) -> bool {
        left.x.relative_eq(&right.x, epsilon, max_relative)
            && left.y.relative_eq(&right.y, epsilon, max_relative)
            && left.z.relative_eq(&right.z, epsilon, max_relative)
    }

    fn abs_diff_eq_vec3x4(left: &Vec3x4, right: &Vec3x4, epsilon: f32) -> bool {
        left.x.abs_diff_eq(&right.x, epsilon)
            && left.y.abs_diff_eq(&right.y, epsilon)
            && left.z.abs_diff_eq(&right.z, epsilon)
    }

    fn relative_eq_uint_vec3x4(
        left: &UnitVec3x4,
        right: &UnitVec3x4,
        epsilon: f32,
        max_relative: f32,
    ) -> bool {
        left.x.relative_eq(&right.x, epsilon, max_relative)
            && left.y.relative_eq(&right.y, epsilon, max_relative)
            && left.z.relative_eq(&right.z, epsilon, max_relative)
    }

    fn abs_diff_eq_uint_vec3x4(left: &UnitVec3x4, right: &UnitVec3x4, epsilon: f32) -> bool {
        left.x.abs_diff_eq(&right.x, epsilon)
            && left.y.abs_diff_eq(&right.y, epsilon)
            && left.z.abs_diff_eq(&right.z, epsilon)
    }
}