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 std::cmp::max;
use std::ops::{Mul, SubAssign};

use approx_det::{assert_relative_eq, AbsDiffEq, RelativeEq};
use glam_det::nums::f32x4;
use glam_det::nums::num_traits::*;
use glam_det::{Point3, UnitQuat, UnitQuatx4, Vec3, Vec3x4};
use phys_geom::shape::{Capsule, Cuboid, Cylinder, Sphere};
use serde::{Deserialize, Serialize};

use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::traits::{ArrayGetter, CreateShapeWide};
use crate::{
    ContactManifold, ConvexContactManifold, ConvexHull, PairTest, ShapeContainer, ShapeTester,
    Triangle,
};

#[derive(Debug, Serialize, Deserialize, PartialEq, Clone)]
pub struct TestInput<ShapeA, ShapeB> {
    pub a: ShapeA,
    pub b: ShapeB,
    pub speculative_margin: f32,
    pub offset_b: Vec3,
    pub orientation_a: UnitQuat,
    pub orientation_b: UnitQuat,
}

#[derive(Debug)]
pub struct TestInputWide<ShapeWideA, ShapeWideB> {
    pub a: ShapeWideA,
    pub b: ShapeWideB,
    pub speculative_margin: f32x4,
    pub offset_b: Vec3x4,
    pub orientation_a: UnitQuatx4,
    pub orientation_b: UnitQuatx4,
}

#[derive(Debug, Default, serde::Serialize, serde::Deserialize)]
pub struct ConvexHullInput {
    pub vertices: Vec<Point3>,
}

#[must_use]
pub fn get_input_wide<
    ShapeWideA: Default + CreateShapeWide<4> + CreateShapeWide<3> + CreateShapeWide<2> + CreateShapeWide<1>,
    ShapeWideB: Default + CreateShapeWide<4> + CreateShapeWide<3> + CreateShapeWide<2> + CreateShapeWide<1>,
>(
    inputs: &[TestInput<ShapeWideA::TShape, ShapeWideB::TShape>],
) -> TestInputWide<ShapeWideA, ShapeWideB> {
    let pair_count = inputs.len();

    assert!(pair_count <= 4);

    let mut a = ShapeWideA::default();
    let mut b = ShapeWideB::default();
    let mut speculative_margin = f32x4::ZERO;
    let mut offset_b = Vec3x4::ZERO;
    let mut orientation_a = UnitQuatx4::default();
    let mut orientation_b = UnitQuatx4::default();
    let array = inputs;
    match pair_count {
        4 => {
            a = <ShapeWideA as CreateShapeWide<4>>::create(array.iter().map(|x| &x.a));
            b = <ShapeWideB as CreateShapeWide<4>>::create(array.iter().map(|x| &x.b));
            speculative_margin = f32x4::from(ArrayGetter::<4>::get_array4_from_iter(
                array.iter().map(|x| x.speculative_margin),
                0.0f32,
            ));
            offset_b =
                ArrayGetter::<4>::get_vec3x4_from_array4(ArrayGetter::<4>::get_array4_from_iter(
                    array.iter().map(|x| x.offset_b),
                    Vec3::ZERO,
                ));
            orientation_a =
                ArrayGetter::<4>::get_unit_quat3x4_from_iter(array.iter().map(|x| x.orientation_a));
            orientation_b =
                ArrayGetter::<4>::get_unit_quat3x4_from_iter(array.iter().map(|x| x.orientation_b));
        }
        3 => {
            a = <ShapeWideA as CreateShapeWide<3>>::create(array.iter().map(|x| &x.a));
            b = <ShapeWideB as CreateShapeWide<3>>::create(array.iter().map(|x| &x.b));
            speculative_margin = f32x4::from(ArrayGetter::<3>::get_array4_from_iter(
                array.iter().map(|x| x.speculative_margin),
                0.0f32,
            ));
            offset_b =
                ArrayGetter::<3>::get_vec3x4_from_array4(ArrayGetter::<3>::get_array4_from_iter(
                    array.iter().map(
                        #[inline]
                        |x| x.offset_b,
                    ),
                    Vec3::ZERO,
                ));
            orientation_a =
                ArrayGetter::<3>::get_unit_quat3x4_from_iter(array.iter().map(|x| x.orientation_a));
            orientation_b =
                ArrayGetter::<3>::get_unit_quat3x4_from_iter(array.iter().map(|x| x.orientation_b));
        }
        2 => {
            a = <ShapeWideA as CreateShapeWide<2>>::create(array.iter().map(|x| &x.a));
            b = <ShapeWideB as CreateShapeWide<2>>::create(array.iter().map(|x| &x.b));
            speculative_margin = f32x4::from(ArrayGetter::<2>::get_array4_from_iter(
                array.iter().map(|x| x.speculative_margin),
                0.0f32,
            ));
            offset_b =
                ArrayGetter::<2>::get_vec3x4_from_array4(ArrayGetter::<2>::get_array4_from_iter(
                    array.iter().map(
                        #[inline]
                        |x| x.offset_b,
                    ),
                    Vec3::ZERO,
                ));
            orientation_a =
                ArrayGetter::<2>::get_unit_quat3x4_from_iter(array.iter().map(|x| x.orientation_a));
            orientation_b =
                ArrayGetter::<2>::get_unit_quat3x4_from_iter(array.iter().map(|x| x.orientation_b));
        }
        1 => {
            a = <ShapeWideA as CreateShapeWide<1>>::create(array.iter().map(|x| &x.a));
            b = <ShapeWideB as CreateShapeWide<1>>::create(array.iter().map(|x| &x.b));
            speculative_margin = f32x4::from(ArrayGetter::<1>::get_array4_from_iter(
                array.iter().map(|x| x.speculative_margin),
                0.0f32,
            ));
            offset_b =
                ArrayGetter::<1>::get_vec3x4_from_array4(ArrayGetter::<1>::get_array4_from_iter(
                    array.iter().map(
                        #[inline]
                        |x| x.offset_b,
                    ),
                    Vec3::ZERO,
                ));
            orientation_a =
                ArrayGetter::<1>::get_unit_quat3x4_from_iter(array.iter().map(|x| x.orientation_a));
            orientation_b =
                ArrayGetter::<1>::get_unit_quat3x4_from_iter(array.iter().map(|x| x.orientation_b));
        }
        _ => {}
    }
    TestInputWide {
        a,
        b,
        speculative_margin,
        offset_b,
        orientation_a,
        orientation_b,
    }
}

macro_rules! TestWide {
    ($ShapeWideA:ident,$ShapeWideB:ident,$array:ident,$pair_count:ident,$outputs:ident,$extra_function:ident) => {
        TestWide!(
            $ShapeWideA,
            $ShapeWideB,
            $array,
            $pair_count,
            $outputs,
            $extra_function,
            None
        );
    };
    ($ShapeWideA:ident,$ShapeWideB:ident,$array:ident,$pair_count:ident,$outputs:ident,$extra_function:ident,$container:expr) => {
        use crate::collision_tasks::tests::common::CheckIsNormalized;
        let input_wide = get_input_wide::<$ShapeWideA, $ShapeWideB>(&$array[..]);
        let mut manifold_wide = Default::default();
        let contact_context = crate::traits::ContactContext {
            orientation_a: &input_wide.orientation_a,
            orientation_b: &input_wide.orientation_b,
            offset_b: &input_wide.offset_b,
            speculative_margin: input_wide.speculative_margin,
            pair_count: $pair_count,
            complex_shape_container: $container,
        };

        ShapeWideTester::test(
            &input_wide.a,
            &input_wide.b,
            &contact_context,
            &mut manifold_wide,
        );
        (0..$array.len()).for_each(|index| {
            let output_2 = $extra_function(&manifold_wide, index);
            assert_relative_eq!($outputs[index], output_2, epsilon = 1e-4f32);
            output_2.check();
        });
    };
    ($ShapeWideA:ident,$ShapeWideB:ident,$array:ident,$pair_count:ident,$outputs:ident,$extra_function:ident,$container:expr,epsilon = $epsilon:expr) => {
        let input_wide = get_input_wide::<$ShapeWideA, $ShapeWideB>(&$array[..]);
        let mut manifold_wide = Default::default();
        let contact_context = crate::traits::ContactContext {
            orientation_a: &input_wide.orientation_a,
            orientation_b: &input_wide.orientation_b,
            offset_b: &input_wide.offset_b,
            speculative_margin: input_wide.speculative_margin,
            pair_count: $pair_count,
            complex_shape_container: $container,
        };
        ShapeWideTester::test(
            &input_wide.a,
            &input_wide.b,
            &contact_context,
            &mut manifold_wide,
        );
        (0..$array.len()).for_each(|index| {
            let output_2 = $extra_function(&manifold_wide, index);
            assert_relative_eq!($outputs[index], output_2, epsilon = $epsilon);
        });
    };
    ($ShapeWideA:ident,$ShapeWideB:ident,$array:ident,$pair_count:ident,$outputs:ident,$extra_function:ident,$container:expr,$compare:tt) => {
        let input_wide = get_input_wide::<$ShapeWideA, $ShapeWideB>(&$array[..]);
        let mut manifold_wide = Default::default();
        let contact_context = crate::traits::ContactContext {
            orientation_a: &input_wide.orientation_a,
            orientation_b: &input_wide.orientation_b,
            offset_b: &input_wide.offset_b,
            speculative_margin: input_wide.speculative_margin,
            pair_count: $pair_count,
            complex_shape_container: $container,
        };
        ShapeWideTester::test(
            &input_wide.a,
            &input_wide.b,
            &contact_context,
            &mut manifold_wide,
        );
        (0..$array.len()).for_each(|index| {
            let output_2 = $extra_function(&manifold_wide, index);
            ($compare)(&$outputs[index], &output_2);
        });
    };
}

#[derive(Debug, Serialize, Deserialize, PartialEq, Clone, Copy)]
pub struct Mvec3 {
    pub x: f32,
    pub y: f32,
    pub z: f32,
}

impl Mvec3 {
    pub fn is_normalized(&self) {
        let length_sqr = self.x * self.x + self.y * self.y + self.z * self.z;
        assert_relative_eq!(length_sqr.sqrt(), 1.0, epsilon = 1e-5);
    }
}

impl Mul<f32> for Mvec3 {
    type Output = Mvec3;

    fn mul(self, rhs: f32) -> Self::Output {
        Mvec3 {
            x: self.x * rhs,
            y: self.y * rhs,
            z: self.z * rhs,
        }
    }
}

impl SubAssign for Mvec3 {
    fn sub_assign(&mut self, rhs: Self) {
        self.x -= rhs.x;
        self.y -= rhs.y;
        self.z -= rhs.z;
    }
}

impl RelativeEq for Mvec3 {
    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.x.relative_eq(&other.x, epsilon, max_relative)
            && self.y.relative_eq(&other.y, epsilon, max_relative)
            && self.z.relative_eq(&other.z, epsilon, max_relative)
    }
}

impl AbsDiffEq for Mvec3 {
    type Epsilon = f32;

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

    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
        self.x.abs_diff_eq(&other.x, epsilon)
            && self.y.abs_diff_eq(&other.y, epsilon)
            && self.z.abs_diff_eq(&other.z, epsilon)
    }
}

impl From<Mvec3> for Vec3 {
    fn from(val: Mvec3) -> Self {
        Vec3::new(val.x, val.y, val.z)
    }
}

impl From<Vec3> for Mvec3 {
    fn from(vec: Vec3) -> Self {
        Mvec3 {
            x: vec.x,
            y: vec.y,
            z: vec.z,
        }
    }
}

#[derive(Debug, Serialize, Deserialize, PartialEq)]
pub(super) struct MQuat {
    pub x: f32,
    pub y: f32,
    pub z: f32,
    pub w: f32,
}

impl AbsDiffEq for MQuat {
    type Epsilon = f32;

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

    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
        self.x.abs_diff_eq(&other.x, epsilon)
            && self.y.abs_diff_eq(&other.y, epsilon)
            && self.z.abs_diff_eq(&other.z, epsilon)
            && self.w.abs_diff_eq(&other.w, epsilon)
    }
}

pub trait CheckIsNormalized {
    fn check(&self);
}

#[derive(Debug, Serialize, Deserialize, PartialEq)]
// This struct is used for serialization and deserialization, so we need to keep the field names as
// they are.
#[allow(non_snake_case)]
#[allow(clippy::struct_excessive_bools)] // We need to make Convex4ContactManifold consistent with Convex4ContactManifoldWide.
pub struct Convex4ContactManifold {
    pub OffsetA0: Mvec3,
    pub OffsetA1: Mvec3,
    pub OffsetA2: Mvec3,
    pub OffsetA3: Mvec3,
    pub Normal: Mvec3,
    pub Depth0: f32,
    pub Depth1: f32,
    pub Depth2: f32,
    pub Depth3: f32,
    pub FeatureId0: i32,
    pub FeatureId1: i32,
    pub FeatureId2: i32,
    pub FeatureId3: i32,
    pub Contact0Exists: bool,
    pub Contact1Exists: bool,
    pub Contact2Exists: bool,
    pub Contact3Exists: bool,
}

impl Convex4ContactManifold {
    pub fn to_array(&self) -> [Convex1ContactManifold; 4] {
        let e0 = Convex1ContactManifold {
            offset_a: self.OffsetA0,
            normal: self.Normal,
            depth: self.Depth0,
            feature_id: self.FeatureId0,
            contact_exists: self.Contact0Exists,
        };
        let e1 = Convex1ContactManifold {
            offset_a: self.OffsetA1,
            normal: self.Normal,
            depth: self.Depth1,
            feature_id: self.FeatureId1,
            contact_exists: self.Contact1Exists,
        };
        let e2 = Convex1ContactManifold {
            offset_a: self.OffsetA2,
            normal: self.Normal,
            depth: self.Depth2,
            feature_id: self.FeatureId2,
            contact_exists: self.Contact2Exists,
        };
        let e3 = Convex1ContactManifold {
            offset_a: self.OffsetA3,
            normal: self.Normal,
            depth: self.Depth3,
            feature_id: self.FeatureId3,
            contact_exists: self.Contact3Exists,
        };
        [e0, e1, e2, e3]
    }
}

impl CheckIsNormalized for Convex4ContactManifold {
    fn check(&self) {
        self.Normal.is_normalized();
    }
}

impl Default for Convex4ContactManifold {
    fn default() -> Self {
        Self {
            OffsetA0: Mvec3 {
                x: 0.0,
                y: 0.0,
                z: 0.0,
            },
            OffsetA1: Mvec3 {
                x: 0.0,
                y: 0.0,
                z: 0.0,
            },
            OffsetA2: Mvec3 {
                x: 0.0,
                y: 0.0,
                z: 0.0,
            },
            OffsetA3: Mvec3 {
                x: 0.0,
                y: 0.0,
                z: 0.0,
            },
            Normal: Mvec3 {
                x: 1.0,
                y: 0.0,
                z: 0.0,
            },
            Depth0: 0.0,
            Depth1: 0.0,
            Depth2: 0.0,
            Depth3: 0.0,
            FeatureId0: 0,
            FeatureId1: 0,
            FeatureId2: 0,
            FeatureId3: 0,
            Contact0Exists: false,
            Contact1Exists: false,
            Contact2Exists: false,
            Contact3Exists: false,
        }
    }
}

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

    fn relative_eq(
        &self,
        other: &Self,
        epsilon: Self::Epsilon,
        max_relative: Self::Epsilon,
    ) -> bool {
        let mut equal = true;
        let _ = [
            self.FeatureId0,
            self.FeatureId1,
            self.FeatureId2,
            self.FeatureId3,
        ];
        let _ = [
            other.FeatureId0,
            other.FeatureId1,
            other.FeatureId2,
            other.FeatureId3,
        ];
        let depth_self = [self.Depth0, self.Depth1, self.Depth2, self.Depth3];
        let depth_other = [other.Depth0, other.Depth1, other.Depth2, other.Depth3];
        let exists_self = [
            self.Contact0Exists,
            self.Contact1Exists,
            self.Contact2Exists,
            self.Contact3Exists,
        ];
        let exists_other = [
            other.Contact0Exists,
            other.Contact1Exists,
            other.Contact2Exists,
            other.Contact3Exists,
        ];
        {
            let array_self = [self.OffsetA0, self.OffsetA1, self.OffsetA2, self.OffsetA3];
            let array_other = [
                other.OffsetA0,
                other.OffsetA1,
                other.OffsetA2,
                other.OffsetA3,
            ];
            let mut bool_visited = [false, false, false, false];
            for i in 0..4 {
                let mut found = false;
                for j in 0..4 {
                    if !bool_visited[j]
                        && array_self[i].relative_eq(&array_other[j], epsilon, max_relative)
                        && exists_self[i].eq(&exists_other[j])
                        && depth_self[i].relative_eq(&depth_other[j], epsilon, max_relative)
                    {
                        bool_visited[j] = true;
                        found = true;
                        break;
                    }
                }
                if !found {
                    equal = false;
                    break;
                }
            }
        }

        equal
            && self
                .Normal
                .relative_eq(&other.Normal, epsilon, max_relative)
    }
}

impl AbsDiffEq for Convex4ContactManifold {
    type Epsilon = f32;

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

    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
        self.OffsetA0.abs_diff_eq(&other.OffsetA0, epsilon)
            && self.OffsetA1.abs_diff_eq(&other.OffsetA1, epsilon)
            && self.OffsetA2.abs_diff_eq(&other.OffsetA2, epsilon)
            && self.OffsetA3.abs_diff_eq(&other.OffsetA3, epsilon)
            && self.Normal.abs_diff_eq(&other.Normal, epsilon)
            && self.Depth0.abs_diff_eq(&other.Depth0, epsilon)
            && self.Depth1.abs_diff_eq(&other.Depth1, epsilon)
            && self.Depth2.abs_diff_eq(&other.Depth2, epsilon)
            && self.Depth3.abs_diff_eq(&other.Depth3, epsilon)
            && self.FeatureId0 == other.FeatureId0
            && self.FeatureId1 == other.FeatureId1
            && self.FeatureId2 == other.FeatureId2
            && self.FeatureId3 == other.FeatureId3
            && self.Contact0Exists == other.Contact0Exists
            && self.Contact1Exists == other.Contact1Exists
            && self.Contact2Exists == other.Contact2Exists
            && self.Contact3Exists == other.Contact3Exists
    }
}

#[must_use]
pub fn convex4manifold_wide2convex4contact_manifold(
    p0: &Convex4ContactManifoldWide,
    index: usize,
) -> Convex4ContactManifold {
    Convex4ContactManifold {
        OffsetA0: Mvec3 {
            x: p0.offset_a[0].x.extract(index),
            y: p0.offset_a[0].y.extract(index),
            z: p0.offset_a[0].z.extract(index),
        },
        OffsetA1: Mvec3 {
            x: p0.offset_a[1].x.extract(index),
            y: p0.offset_a[1].y.extract(index),
            z: p0.offset_a[1].z.extract(index),
        },
        OffsetA2: Mvec3 {
            x: p0.offset_a[2].x.extract(index),
            y: p0.offset_a[2].y.extract(index),
            z: p0.offset_a[2].z.extract(index),
        },
        OffsetA3: Mvec3 {
            x: p0.offset_a[3].x.extract(index),
            y: p0.offset_a[3].y.extract(index),
            z: p0.offset_a[3].z.extract(index),
        },
        Normal: Mvec3 {
            x: p0.normal.x.extract(index),
            y: p0.normal.y.extract(index),
            z: p0.normal.z.extract(index),
        },
        Depth0: p0.depth[0].extract(index),
        Depth1: p0.depth[1].extract(index),
        Depth2: p0.depth[2].extract(index),
        Depth3: p0.depth[3].extract(index),
        FeatureId0: u32_feature_id_to_i32(p0.feature_id[0].extract(index)),
        FeatureId1: u32_feature_id_to_i32(p0.feature_id[1].extract(index)),
        FeatureId2: u32_feature_id_to_i32(p0.feature_id[2].extract(index)),
        FeatureId3: u32_feature_id_to_i32(p0.feature_id[3].extract(index)),
        Contact0Exists: p0.contact_exists[0].extract(index),
        Contact1Exists: p0.contact_exists[1].extract(index),
        Contact2Exists: p0.contact_exists[2].extract(index),
        Contact3Exists: p0.contact_exists[3].extract(index),
    }
}

impl Convex4ContactManifold {
    pub(super) fn adjust_offset_a_to_shape_surface(&mut self, depth_factor: f32) {
        let normal: Vec3 = self.Normal.into();

        let depth0 = self.Depth0;
        let adjust_offset_a0 = -normal * depth_factor * depth0;
        let original_offset_a0: Vec3 = self.OffsetA0.into();
        self.OffsetA0 = (original_offset_a0 + adjust_offset_a0).into();

        let depth1 = self.Depth1;
        let adjust_offset_a1 = -normal * depth_factor * depth1;
        let original_offset_a1: Vec3 = self.OffsetA1.into();
        self.OffsetA1 = (original_offset_a1 + adjust_offset_a1).into();

        let depth2 = self.Depth2;
        let adjust_offset_a2 = -normal * depth_factor * depth2;
        let original_offset_a2: Vec3 = self.OffsetA2.into();
        self.OffsetA2 = (original_offset_a2 + adjust_offset_a2).into();

        let depth3 = self.Depth3;
        let adjust_offset_a3 = -normal * depth_factor * depth3;
        let original_offset_a3: Vec3 = self.OffsetA3.into();
        self.OffsetA3 = (original_offset_a3 + adjust_offset_a3).into();
    }
}

#[derive(Debug, Serialize, Deserialize, PartialEq)]
#[allow(non_snake_case)] // This struct is used for serialization and deserialization, so we need to keep the field names as
                         // they are.
pub struct Convex2ContactManifold {
    pub OffsetA0: Mvec3,
    pub OffsetA1: Mvec3,
    pub Normal: Mvec3,
    pub Depth0: f32,
    pub Depth1: f32,
    pub FeatureId0: i32,
    pub FeatureId1: i32,
    pub Contact0Exists: bool,
    pub Contact1Exists: bool,
}

impl CheckIsNormalized for Convex2ContactManifold {
    fn check(&self) {
        self.Normal.is_normalized();
    }
}

impl Default for Convex2ContactManifold {
    fn default() -> Self {
        Self {
            OffsetA0: Mvec3 {
                x: 0.0,
                y: 0.0,
                z: 0.0,
            },
            OffsetA1: Mvec3 {
                x: 0.0,
                y: 0.0,
                z: 0.0,
            },
            Normal: Mvec3 {
                x: 1.0,
                y: 0.0,
                z: 0.0,
            },
            Depth0: 0f32,
            Depth1: 0f32,
            FeatureId0: 0,
            FeatureId1: 0,
            Contact0Exists: false,
            Contact1Exists: false,
        }
    }
}

impl RelativeEq for Convex2ContactManifold {
    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.OffsetA0
            .relative_eq(&other.OffsetA0, epsilon, max_relative)
            && self
                .OffsetA1
                .relative_eq(&other.OffsetA1, epsilon, max_relative)
            && self
                .Normal
                .relative_eq(&other.Normal, epsilon, max_relative)
            && self
                .Depth0
                .relative_eq(&other.Depth0, epsilon, max_relative)
            && self
                .Depth1
                .relative_eq(&other.Depth1, epsilon, max_relative)
            && self.FeatureId0 == (other.FeatureId0)
            && self.FeatureId1 == (other.FeatureId1)
            && self.Contact0Exists == (other.Contact0Exists)
            && self.Contact1Exists == (other.Contact1Exists)
    }
}

impl AbsDiffEq for Convex2ContactManifold {
    type Epsilon = f32;

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

    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
        self.OffsetA0.abs_diff_eq(&other.OffsetA0, epsilon)
            && self.OffsetA1.abs_diff_eq(&other.OffsetA1, epsilon)
            && self.Normal.abs_diff_eq(&other.Normal, epsilon)
            && self.Depth0.abs_diff_eq(&other.Depth0, epsilon)
            && self.Depth1.abs_diff_eq(&other.Depth1, epsilon)
            && self.FeatureId0 == other.FeatureId0
            && self.FeatureId1 == other.FeatureId1
            && self.Contact0Exists == other.Contact0Exists
            && self.Contact1Exists == other.Contact1Exists
    }
}

#[inline]
#[allow(clippy::cast_possible_wrap)]
fn u32_feature_id_to_i32(feature_id: u32) -> i32 {
    if feature_id > i32::MAX as u32 {
        -((u32::MAX - feature_id) as i32)
    } else {
        feature_id as i32
    }
}

#[must_use]
pub fn convex2contact_manifold_wide_to_convex2contact_manifold(
    p0: &Convex4ContactManifoldWide,
    index: usize,
) -> Convex2ContactManifold {
    Convex2ContactManifold {
        OffsetA0: Mvec3 {
            x: p0.offset_a[0].x.extract(index),
            y: p0.offset_a[0].y.extract(index),
            z: p0.offset_a[0].z.extract(index),
        },
        OffsetA1: Mvec3 {
            x: p0.offset_a[1].x.extract(index),
            y: p0.offset_a[1].y.extract(index),
            z: p0.offset_a[1].z.extract(index),
        },
        Normal: Mvec3 {
            x: p0.normal.x.extract(index),
            y: p0.normal.y.extract(index),
            z: p0.normal.z.extract(index),
        },
        Depth0: p0.depth[0].extract(index),
        Depth1: p0.depth[1].extract(index),
        FeatureId0: u32_feature_id_to_i32(p0.feature_id[0].extract(index)),
        FeatureId1: u32_feature_id_to_i32(p0.feature_id[1].extract(index)),
        Contact0Exists: p0.contact_exists[0].extract(index),
        Contact1Exists: p0.contact_exists[1].extract(index),
    }
}

impl Convex2ContactManifold {
    pub(super) fn adjust_offset_a_to_shape_surface(&mut self, depth_factor: f32) {
        let normal: Vec3 = self.Normal.into();
        let depth0 = self.Depth0;
        let adjust_offset_a0 = -normal * depth_factor * depth0;
        let original_offset_a0: Vec3 = self.OffsetA0.into();
        let offset_a0 = original_offset_a0 + adjust_offset_a0;

        let depth1 = self.Depth1;
        let adjust_offset_a1 = -normal * depth_factor * depth1;
        let original_offset_a1: Vec3 = self.OffsetA1.into();
        let offset_a1 = original_offset_a1 + adjust_offset_a1;

        self.OffsetA0 = offset_a0.into();
        self.OffsetA1 = offset_a1.into();
    }
}

#[derive(Debug, Serialize, Deserialize, PartialEq)]
pub struct Convex1ContactManifold {
    pub offset_a: Mvec3,
    pub normal: Mvec3,
    pub depth: f32,
    pub feature_id: i32,
    pub contact_exists: bool,
}

impl CheckIsNormalized for Convex1ContactManifold {
    fn check(&self) {
        self.normal.is_normalized();
    }
}

#[must_use]
pub fn convex1contact_manifold_wide_to_convex1contact_manifold(
    p0: &Convex4ContactManifoldWide,
    index: usize,
) -> Convex1ContactManifold {
    Convex1ContactManifold {
        offset_a: Mvec3 {
            x: p0.offset_a[0].x.extract(index),
            y: p0.offset_a[0].y.extract(index),
            z: p0.offset_a[0].z.extract(index),
        },
        normal: Mvec3 {
            x: p0.normal.x.extract(index),
            y: p0.normal.y.extract(index),
            z: p0.normal.z.extract(index),
        },
        depth: p0.depth[0].extract(index),
        feature_id: u32_feature_id_to_i32(p0.feature_id[0].extract(index)),
        contact_exists: p0.contact_exists[0].extract(index),
    }
}

impl RelativeEq for Convex1ContactManifold {
    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
            .relative_eq(&other.offset_a, epsilon, max_relative)
            && self
                .normal
                .relative_eq(&other.normal, epsilon, max_relative)
            && self.depth.relative_eq(&other.depth, epsilon, max_relative)
            && self.feature_id == (other.feature_id)
            && self.contact_exists == (other.contact_exists)
    }
}

impl AbsDiffEq for Convex1ContactManifold {
    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.abs_diff_eq(&other.offset_a, epsilon)
            && self.normal.abs_diff_eq(&other.normal, epsilon)
            && self.depth.abs_diff_eq(&other.depth, epsilon)
            && self.feature_id == (other.feature_id)
            && self.contact_exists == (other.contact_exists)
    }
}

impl Convex1ContactManifold {
    pub(super) fn adjust_offset_a_to_shape_surface(&mut self, depth_factor: f32) {
        let normal: Vec3 = self.normal.into();
        let depth = self.depth;
        let adjust_offset_a = -normal * depth_factor * depth;
        let original_offset_a: Vec3 = self.offset_a.into();
        let offset_a = original_offset_a + adjust_offset_a;

        self.offset_a = offset_a.into();
    }
}

#[allow(dead_code)]
pub(super) enum CollisionTestShape {
    Cuboid(Cuboid),
    Sphere(Sphere),
    Capsule(Capsule),
    Cylinder(Cylinder),
    Triangle(Triangle),
}

pub(super) fn compare_and_print_convex_contact_manifold(
    manifold_expect: &ConvexContactManifold,
    manifold_actual: &ConvexContactManifold,
) -> f32 {
    let mut max_difference_err: f32 = 0.0;

    if manifold_expect.contact_count() != manifold_actual.contact_count() {
        max_difference_err = 1.0;
    }
    println!(
        "contact_count: {:?}, {:?}",
        manifold_expect.contact_count(),
        manifold_actual.contact_count()
    );

    let normal_distance_err = manifold_expect.normal.distance(manifold_actual.normal);
    max_difference_err = max_difference_err.max(normal_distance_err);
    println!(
        "normal: {:?}, {:?}, err: {:.2}",
        manifold_expect.normal, manifold_actual.normal, normal_distance_err
    );

    for i in 0..max(
        manifold_expect.contact_count(),
        manifold_actual.contact_count(),
    ) {
        let offset_a_err = manifold_expect
            .get_contact_offset_a(i)
            .distance(manifold_actual.get_contact_offset_a(i));
        max_difference_err = max_difference_err.max(offset_a_err);

        println!(
            "contact[{i}].offset_a:\t{:?}, {:?}; err: {:.2}",
            manifold_expect.get_contact_offset_a(i),
            manifold_actual.get_contact_offset_a(i),
            offset_a_err
        );

        let depth_err =
            (manifold_expect.get_contact_depth(i) - manifold_actual.get_contact_depth(i)).abs();
        max_difference_err = max_difference_err.max(depth_err);

        println!(
            "contact[{i}].depth:\t{:?}, {:?}; err: {:.2}",
            manifold_expect.get_contact_depth(i),
            manifold_actual.get_contact_depth(i),
            depth_err
        );
    }

    max_difference_err
}

fn execute_shape_displaced_convexhull_test(
    shape_a: &CollisionTestShape,
    convexhull_cuboid_length: Vec3,
    convexhull_displacement_vec: Vec3,
    shift_align_center: bool,
    offset_b: Vec3,
    orientation_a: UnitQuat,
    orientation_b: UnitQuat,
) -> ConvexContactManifold {
    let points = ConvexHull::generate_displacement_cuboid_vertex(
        convexhull_cuboid_length,
        convexhull_displacement_vec,
    );

    let convex_hull = if shift_align_center {
        ConvexHull::new_unchecked_and_shift(&points)
    } else {
        ConvexHull::new_unchecked(&points)
    };
    let mut container = ShapeContainer::default();
    let hull_id = container.add(convex_hull);

    match shape_a {
        CollisionTestShape::Cuboid(shape_a) => ShapeTester::test(
            *shape_a,
            hull_id,
            1e-5,
            offset_b,
            orientation_a,
            orientation_b,
            Some(&container),
        ),
        CollisionTestShape::Sphere(shape_a) => ShapeTester::test(
            *shape_a,
            hull_id,
            1e-5,
            offset_b,
            orientation_a,
            orientation_b,
            Some(&container),
        ),
        CollisionTestShape::Capsule(shape_a) => ShapeTester::test(
            *shape_a,
            hull_id,
            1e-5,
            offset_b,
            orientation_a,
            orientation_b,
            Some(&container),
        ),
        CollisionTestShape::Cylinder(shape_a) => ShapeTester::test(
            *shape_a,
            hull_id,
            1e-5,
            offset_b,
            orientation_a,
            orientation_b,
            Some(&container),
        ),
        CollisionTestShape::Triangle(shape_a) => ShapeTester::test(
            *shape_a,
            hull_id,
            1e-5,
            offset_b,
            orientation_a,
            orientation_b,
            Some(&container),
        ),
    }
}

pub(super) fn build_displaced_convexhull_test(
    shape_a: &CollisionTestShape,
    convexhull_cuboid_length: Vec3,
    convexhull_displacement_vec: Vec3,
    desired_offset_b: Vec3,
    rotation_a: UnitQuat,
    rotation_b: UnitQuat,
) {
    let _ = env_logger::builder().is_test(true).try_init();

    println!("convexhull_displacement_vec: {convexhull_displacement_vec}; rotation_a: {rotation_a}, rotation_b: {rotation_b}");

    let manifold_native_centering = execute_shape_displaced_convexhull_test(
        shape_a,
        convexhull_cuboid_length,
        Vec3::ZERO,
        false,
        desired_offset_b,
        rotation_a,
        rotation_b,
    );

    let rotated_translate_vec = rotation_b * convexhull_displacement_vec;
    let adjusted_offset_b = desired_offset_b - rotated_translate_vec;

    let manifold_with_translated = execute_shape_displaced_convexhull_test(
        shape_a,
        convexhull_cuboid_length,
        convexhull_displacement_vec,
        false,
        adjusted_offset_b,
        rotation_a,
        rotation_b,
    );
    let manifold_with_translated_and_shift_align_center = execute_shape_displaced_convexhull_test(
        shape_a,
        convexhull_cuboid_length,
        convexhull_displacement_vec,
        true,
        adjusted_offset_b,
        rotation_a,
        rotation_b,
    );

    println!("shift_align_center OFF:");
    let err_no_align = compare_and_print_convex_contact_manifold(
        &manifold_native_centering,
        &manifold_with_translated,
    );
    println!("shift_align_center ON:");
    let err_with_align = compare_and_print_convex_contact_manifold(
        &manifold_native_centering,
        &manifold_with_translated_and_shift_align_center,
    );

    println!(
        "Max error when shift_align_center OFF: {err_no_align:.2}; Max error when shift_align_center ON: {err_with_align:.2}"
    );

    assert!(err_no_align >= err_with_align);
    assert_eq!(
        manifold_native_centering.count,
        manifold_with_translated_and_shift_align_center.count
    );
}