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};
use glam_det::{UnitQuat, UnitQuatx4, Vec3, Vec3x4};

use crate::collision_tasks::ReductionId;
use crate::shapes::{Sphere, SphereWide};
use crate::traits::{
    ArrayGetter, BaseShapeWide, CollisionPairWide, CreateShapeWide, PairBatch, PairWide,
    WritePairWide,
};

macro_rules! define_pair_struct {
    ($struct_name: ident,$struct_sub_name: ident,TwoShape) => {
        #[derive(Clone)]
        pub struct $struct_name<TShapeA: Default, TShapeB: Default> {
            pairs: Vec<$struct_sub_name<TShapeA, TShapeB>>,
        }
    };
    ($struct_name: ident,$struct_sub_name: ident,OneShape) => {
        #[derive(Clone)]
        pub struct $struct_name<TShape: Default> {
            pairs: Vec<$struct_sub_name<TShape>>,
        }
    };
    ($struct_name: ident,$struct_sub_name: ident) => {
        #[derive(Clone)]
        pub struct $struct_name {
            pairs: Vec<$struct_sub_name>,
        }
    };
}

macro_rules! impl_pair_narrowphase {
    ($shape0: ident,$shape1: ident,$shape_wide0: ident,$shape_wide1: ident,$max_contact_count:expr) => {
        impl PairTest<$shape0, $shape1, ConvexContactManifold> for ShapeTester {
            fn test(
                a: $shape0,
                b: $shape1,
                speculative_margin: f32,
                offset_b: Vec3,
                orientation_a: UnitQuat,
                orientation_b: UnitQuat,
                container: Option<&ShapeContainer>,
            ) -> ConvexContactManifold {
                let a = <$shape_wide0 as CreateShapeWide<1>>::create([a].iter());
                let b = <$shape_wide1 as CreateShapeWide<1>>::create([b].iter());
                let speculative_margin = f32x4::splat(speculative_margin);
                let offset_b = Vec3x4::new(
                    f32x4::splat(offset_b.x),
                    f32x4::splat(offset_b.y),
                    f32x4::splat(offset_b.z),
                );
                let orientation_a = UnitQuatx4::from_xyzw_unchecked(
                    f32x4::splat(orientation_a.x),
                    f32x4::splat(orientation_a.y),
                    f32x4::splat(orientation_a.z),
                    f32x4::splat(orientation_a.w),
                );
                let orientation_b = UnitQuatx4::from_xyzw_unchecked(
                    f32x4::splat(orientation_b.x),
                    f32x4::splat(orientation_b.y),
                    f32x4::splat(orientation_b.z),
                    f32x4::splat(orientation_b.w),
                );
                let mut manifold = Default::default();
                let contact_context = ContactContext {
                    orientation_a: &orientation_a,
                    orientation_b: &orientation_b,
                    offset_b: &offset_b,
                    speculative_margin,
                    pair_count: 1,
                    complex_shape_container: container,
                };
                ShapeWideTester::test(&a, &b, &contact_context, &mut manifold);
                let mut result = ConvexContactManifold::default();
                ContactManifoldWide::<ConvexContactManifold>::get_manifold(
                    &manifold,
                    0,
                    $max_contact_count,
                    &offset_b,
                    &mut result,
                );
                result
            }
        }
    };
}

macro_rules! impl_pair_batch {
    ( $pair_type_ident: ident, $pair_batch_type:ident) => {
        impl PairBatch<$pair_type_ident> for $pair_batch_type {
            #[inline]
            fn get_pairs(&self) -> &[$pair_type_ident] {
                &self.pairs
            }

            #[inline]
            fn push(&mut self, pair: $pair_type_ident) {
                self.pairs.push(pair)
            }

            #[inline]
            fn new(size: usize) -> Self {
                Self {
                    pairs: Vec::with_capacity(size),
                }
            }

            #[inline]
            fn clear(&mut self) {
                self.pairs.clear();
            }
        }
    };
    ( $shape_type0: ident, $shape_type1: ident, $pair_type:ident, $pair_batch_type:ident) => {
        impl<$shape_type0: Default, $shape_type1: Default>
            PairBatch<$pair_type<$shape_type0, $shape_type1>>
            for $pair_batch_type<$shape_type0, $shape_type1>
        {
            #[inline]
            fn get_pairs(&self) -> &[$pair_type<$shape_type0, $shape_type1>] {
                &self.pairs
            }

            #[inline]
            fn push(&mut self, pair: $pair_type<$shape_type0, $shape_type1>) {
                self.pairs.push(pair)
            }

            #[inline]
            fn new(size: usize) -> Self {
                Self {
                    pairs: Vec::with_capacity(size),
                }
            }

            #[inline]
            fn clear(&mut self) {
                self.pairs.clear();
            }
        }
    };
    ( $shape_type0: ident, $pair_type:ident, $pair_batch_type:ident) => {
        impl<$shape_type0: Default> PairBatch<$pair_type<$shape_type0>>
            for $pair_batch_type<$shape_type0>
        {
            #[inline]
            fn get_pairs(&self) -> &[$pair_type<$shape_type0>] {
                &self.pairs
            }

            #[inline]
            fn push(&mut self, pair: $pair_type<$shape_type0>) {
                self.pairs.push(pair)
            }

            #[inline]
            fn new(size: usize) -> Self {
                Self {
                    pairs: Vec::with_capacity(size),
                }
            }

            #[inline]
            fn clear(&mut self) {
                self.pairs.clear();
            }
        }
    };
}

#[derive(Clone)]
pub struct StandardPair<TShapeA: Default, TShapeB: Default> {
    pub a: TShapeA,
    pub b: TShapeB,
    pub flip_mask: bool,
    pub offset_b: Vec3,
    pub orientation_a: UnitQuat,
    pub orientation_b: UnitQuat,
    pub speculative_margin: f32,
    pub reduction_id: ReductionId,
}

pub struct StandardPairWide<TShapeWideA: Default, TShapeWideB: Default> {
    pub a: TShapeWideA,
    pub b: TShapeWideB,
    pub flip_mask: bool32x4,
    pub offset_b: Vec3x4,
    pub orientation_a: UnitQuatx4,
    pub orientation_b: UnitQuatx4,
    pub speculative_margin: f32x4,
    pub reduction_id: [ReductionId; 4],
}

impl<TShapeWideA: Default, TShapeWideB: Default> Default
    for StandardPairWide<TShapeWideA, TShapeWideB>
{
    fn default() -> Self {
        Self {
            a: Default::default(),
            b: Default::default(),
            flip_mask: bool32x4::FALSE,
            offset_b: Vec3x4::default(),
            orientation_a: UnitQuatx4::default(),
            orientation_b: UnitQuatx4::default(),
            speculative_margin: f32x4::ZERO,
            reduction_id: [ReductionId::invalid(); 4],
        }
    }
}

impl<TShapeWideA: BaseShapeWide + Default, TShapeWideB: BaseShapeWide + Default> PairWide
    for StandardPairWide<TShapeWideA, TShapeWideB>
{
    type TPair = StandardPair<TShapeWideA::TShape, TShapeWideB::TShape>;
}

macro_rules! impl_writes_for_standard_pair_wide {
    ($($num:tt),*) => {
        $(
            impl<
                TShapeWideA: CreateShapeWide<$num> + Default,
                TShapeWideB: CreateShapeWide<$num> + Default,
            >
            WritePairWide<$num> for StandardPairWide<TShapeWideA, TShapeWideB> {
                fn write_wide(&mut self, array_slice: [&Self::TPair; $num]) {
                    self.a = TShapeWideA::create(array_slice.iter().map(#[inline]|pair| &pair.a));
                    self.b = TShapeWideB::create(array_slice.iter().map(#[inline]|pair| &pair.b));
                    self.flip_mask = bool32x4::from(ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.flip_mask }), false));
                    self.offset_b = ArrayGetter::<$num>::get_vec3x4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.offset_b }));

                    self.orientation_a =
                        ArrayGetter::<$num>::get_unit_quat3x4_from_iter(array_slice.iter().map(#[inline]|pairs| { pairs.orientation_a }));
                    self.orientation_b =
                        ArrayGetter::<$num>::get_unit_quat3x4_from_iter(array_slice.iter().map(#[inline]|pairs| { pairs.orientation_b }));
                    self.speculative_margin =
                        f32x4::from(ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.speculative_margin }), 0.0));
                    self.reduction_id =  ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.reduction_id }), ReductionId::invalid());
                }
            }
        )*
    };
}

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

impl<
        TShapeWideA: CreateShapeWide<4> + CreateShapeWide<3> + CreateShapeWide<2> + CreateShapeWide<1> + Default,
        TShapeWideB: CreateShapeWide<4> + CreateShapeWide<3> + CreateShapeWide<2> + CreateShapeWide<1> + Default,
    > CollisionPairWide for StandardPairWide<TShapeWideA, TShapeWideB>
{
    type TShapeWideA = TShapeWideA;
    type TShapeWideB = TShapeWideB;

    #[inline]
    fn flip_mask(&self) -> &bool32x4 {
        &self.flip_mask
    }

    #[inline]
    fn speculative_margin(&self) -> &f32x4 {
        &self.speculative_margin
    }

    #[inline]
    fn shape_a(&self) -> &TShapeWideA {
        &self.a
    }

    #[inline]
    fn shape_b(&self) -> &TShapeWideB {
        &self.b
    }

    #[inline]
    fn orientation_a(&self) -> &UnitQuatx4 {
        &self.orientation_a
    }

    #[inline]
    fn orientation_b(&self) -> &UnitQuatx4 {
        &self.orientation_b
    }

    #[inline]
    fn offset_b(&self) -> &Vec3x4 {
        &self.offset_b
    }

    #[inline]
    fn set_offset_b(&mut self, value: Vec3x4) {
        self.offset_b = value;
    }

    #[inline]
    fn reduction_id(&self) -> &[ReductionId; 4] {
        &self.reduction_id
    }
}

impl<TShapeA: Default, TShapeB: Default> Default for StandardPair<TShapeA, TShapeB> {
    fn default() -> Self {
        StandardPair {
            a: Default::default(),
            b: Default::default(),
            flip_mask: false,
            offset_b: Vec3::default(),
            orientation_a: UnitQuat::default(),
            orientation_b: UnitQuat::default(),
            speculative_margin: 0.0,
            reduction_id: ReductionId::invalid(),
        }
    }
}

define_pair_struct!(StandardPairBatch, StandardPair, TwoShape);

impl_pair_batch!(TShapeA, TShapeB, StandardPair, StandardPairBatch);

#[derive(Default, Clone)]
pub struct FliplessPair<TShape: Default> {
    pub a: TShape,
    pub b: TShape,
    pub offset_b: Vec3,
    pub orientation_a: UnitQuat,
    pub orientation_b: UnitQuat,
    pub speculative_margin: f32,
    pub reduction_id: ReductionId,
}

pub struct FliplessPairWide<TShapeWide: BaseShapeWide + Default> {
    a: TShapeWide,
    b: TShapeWide,
    offset_b: Vec3x4,
    orientation_a: UnitQuatx4,
    orientation_b: UnitQuatx4,
    speculative_margin: f32x4,
    reduction_id: [ReductionId; 4],
}

impl<TShapeWide: BaseShapeWide + Default> Default for FliplessPairWide<TShapeWide> {
    fn default() -> Self {
        Self {
            a: Default::default(),
            b: Default::default(),
            offset_b: Vec3x4::default(),
            orientation_a: UnitQuatx4::default(),
            orientation_b: UnitQuatx4::default(),
            speculative_margin: f32x4::ZERO,
            reduction_id: [ReductionId::invalid(); 4],
        }
    }
}

impl<TShapeWide: BaseShapeWide + Default> PairWide for FliplessPairWide<TShapeWide> {
    type TPair = FliplessPair<TShapeWide::TShape>;
}

macro_rules! impl_writes_for_flipless_pair_wide {
    ($($num:tt),*) => {
        $(
            impl<TShapeWide: CreateShapeWide<$num> + Default>
            WritePairWide<$num> for FliplessPairWide<TShapeWide> {
                fn write_wide(&mut self, array_slice: [&Self::TPair; $num]) {
                    self.a = TShapeWide::create(array_slice.iter().map(#[inline]|pair| &pair.a));
                    self.b = TShapeWide::create(array_slice.iter().map(#[inline]|pair| &pair.b));
                    self.offset_b = ArrayGetter::<$num>::get_vec3x4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.offset_b }));

                    self.orientation_a =
                        ArrayGetter::<$num>::get_unit_quat3x4_from_iter(array_slice.iter().map(#[inline]|pairs| { pairs.orientation_a }));
                    self.orientation_b =
                        ArrayGetter::<$num>::get_unit_quat3x4_from_iter(array_slice.iter().map(#[inline]|pairs| { pairs.orientation_b }));
                    self.speculative_margin =
                        f32x4::from(ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.speculative_margin }), 0.0));
                    self.reduction_id =  ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.reduction_id }), ReductionId::invalid());
                }
            }
        )*
    };
}

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

impl<
        TShapeWide: CreateShapeWide<4> + CreateShapeWide<3> + CreateShapeWide<2> + CreateShapeWide<1> + Default,
    > CollisionPairWide for FliplessPairWide<TShapeWide>
{
    type TShapeWideA = TShapeWide;
    type TShapeWideB = TShapeWide;

    #[inline]
    fn flip_mask(&self) -> &bool32x4 {
        unreachable!();
    }

    #[inline]
    fn speculative_margin(&self) -> &f32x4 {
        &self.speculative_margin
    }

    #[inline]
    fn shape_a(&self) -> &TShapeWide {
        &self.a
    }

    #[inline]
    fn shape_b(&self) -> &TShapeWide {
        &self.b
    }

    #[inline]
    fn orientation_a(&self) -> &UnitQuatx4 {
        &self.orientation_a
    }

    #[inline]
    fn orientation_b(&self) -> &UnitQuatx4 {
        &self.orientation_b
    }

    #[inline]
    fn offset_b(&self) -> &Vec3x4 {
        &self.offset_b
    }

    #[inline]
    fn set_offset_b(&mut self, value: Vec3x4) {
        self.offset_b = value;
    }

    #[inline]
    fn reduction_id(&self) -> &[ReductionId; 4] {
        &self.reduction_id
    }
}

define_pair_struct!(FliplessPairBatch, FliplessPair, OneShape);

impl_pair_batch!(TShape, FliplessPair, FliplessPairBatch);

#[derive(Clone)]
pub struct SpherePair {
    pub shape_a: Sphere,
    pub shape_b: Sphere,
    pub offset_b: Vec3,
    pub speculative_margin: f32,
    pub reduction_id: ReductionId,
}

pub struct SpherePairWide {
    shape_a: SphereWide,
    shape_b: SphereWide,
    offset_b: Vec3x4,
    speculative_margin: f32x4,
    reduction_id: [ReductionId; 4],
}

impl Default for SpherePairWide {
    fn default() -> Self {
        Self {
            shape_a: SphereWide::default(),
            shape_b: SphereWide::default(),
            offset_b: Vec3x4::ZERO,
            speculative_margin: f32x4::ZERO,
            reduction_id: [ReductionId::invalid(); 4],
        }
    }
}

impl PairWide for SpherePairWide {
    type TPair = SpherePair;
}

macro_rules! impl_writes_for_sphere_pair_wide {
    ($($num:tt),*) => {
        $(
           impl WritePairWide<$num> for SpherePairWide {
                fn write_wide(&mut self, array_slice: [&Self::TPair; $num]) {
                    self.shape_a = <SphereWide as CreateShapeWide<$num>>::create(array_slice.iter().map(#[inline]|pair| &pair.shape_a));
                    self.shape_b = <SphereWide as CreateShapeWide<$num>>::create(array_slice.iter().map(#[inline]|pair| &pair.shape_b));
                    self.offset_b = ArrayGetter::<$num>::get_vec3x4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.offset_b }));
                    self.speculative_margin =
                        f32x4::from(ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.speculative_margin }), 0.0));
                    self.reduction_id =  ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.reduction_id }), ReductionId::invalid());
                }
            }

        )*
    };
}

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

impl CollisionPairWide for SpherePairWide {
    type TShapeWideA = SphereWide;
    type TShapeWideB = SphereWide;

    #[inline]
    fn flip_mask(&self) -> &bool32x4 {
        unreachable!()
    }

    #[inline]
    fn speculative_margin(&self) -> &f32x4 {
        &self.speculative_margin
    }

    #[inline]
    fn shape_a(&self) -> &SphereWide {
        &self.shape_a
    }

    #[inline]
    fn shape_b(&self) -> &SphereWide {
        &self.shape_b
    }

    #[inline]
    fn orientation_a(&self) -> &UnitQuatx4 {
        &UnitQuatx4::IDENTITY
    }

    #[inline]
    fn orientation_b(&self) -> &UnitQuatx4 {
        &UnitQuatx4::IDENTITY
    }

    #[inline]
    fn offset_b(&self) -> &Vec3x4 {
        &self.offset_b
    }

    #[inline]
    fn set_offset_b(&mut self, value: Vec3x4) {
        self.offset_b = value;
    }

    #[inline]
    fn reduction_id(&self) -> &[ReductionId; 4] {
        &self.reduction_id
    }
}

impl Default for SpherePair {
    fn default() -> Self {
        Self {
            shape_a: Sphere::default(),
            shape_b: Sphere::default(),
            offset_b: Vec3::default(),
            speculative_margin: 0.0,
            reduction_id: ReductionId::invalid(),
        }
    }
}

define_pair_struct!(SpherePairBatch, SpherePair);

impl_pair_batch!(SpherePair, SpherePairBatch);

#[derive(Debug, Clone)]
pub struct SphereIncludingPair<TShapeB: Default> {
    pub a: Sphere,
    pub b: TShapeB,

    pub flip_mask: bool,
    pub offset_b: Vec3,
    pub orientation_b: UnitQuat,
    pub speculative_margin: f32,
    pub reduction_id: ReductionId,
}

#[derive(Debug)]
pub struct SphereIncludingPairWide<TShapeBWide: BaseShapeWide + Default> {
    a: SphereWide,
    b: TShapeBWide,

    flip_mask: bool32x4,
    offset_b: Vec3x4,
    orientation_b: UnitQuatx4,
    speculative_margin: f32x4,
    reduction_id: [ReductionId; 4],
}

impl<TShapeBWide: BaseShapeWide + Default> Default for SphereIncludingPairWide<TShapeBWide> {
    fn default() -> Self {
        Self {
            a: SphereWide::default(),
            b: TShapeBWide::default(),
            flip_mask: bool32x4::FALSE,
            offset_b: Vec3x4::default(),
            orientation_b: UnitQuatx4::default(),
            speculative_margin: f32x4::ZERO,
            reduction_id: [ReductionId::invalid(); 4],
        }
    }
}

impl<TShapeBWide: BaseShapeWide + Default> PairWide for SphereIncludingPairWide<TShapeBWide> {
    type TPair = SphereIncludingPair<TShapeBWide::TShape>;
}

macro_rules! impl_writes_for_sphere_including_pair_wide {
    ($($num:tt),*) => {
        $(
            impl<TShapeBWide: CreateShapeWide<$num> + Default>
            WritePairWide<$num> for SphereIncludingPairWide<TShapeBWide> {
                fn write_wide(&mut self, array_slice: [&Self::TPair; $num]) {
                    self.a = <SphereWide as CreateShapeWide<$num>>::create(array_slice.iter().map(#[inline]|pair| &pair.a));
                    self.b = TShapeBWide::create(array_slice.iter().map(#[inline]|pair| &pair.b));
                    self.flip_mask = bool32x4::from(ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.flip_mask }), false));
                    self.offset_b = ArrayGetter::<$num>::get_vec3x4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.offset_b }));
                    self.orientation_b =
                        ArrayGetter::<$num>::get_unit_quat3x4_from_iter(array_slice.iter().map(#[inline]|pairs| { pairs.orientation_b }));
                    self.speculative_margin =
                        f32x4::from(ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.speculative_margin }), 0.0));
                    self.reduction_id = ArrayGetter::<$num>::get_array4_from_iter(array_slice.iter().map(#[inline]|pair| { pair.reduction_id }), ReductionId::invalid());
                }
            }
        )*
    };
}
impl_writes_for_sphere_including_pair_wide!(1, 2, 3, 4);

impl<
        TShapeBWide: CreateShapeWide<4> + CreateShapeWide<3> + CreateShapeWide<2> + CreateShapeWide<1> + Default,
    > CollisionPairWide for SphereIncludingPairWide<TShapeBWide>
{
    type TShapeWideA = SphereWide;
    type TShapeWideB = TShapeBWide;

    #[inline]
    fn flip_mask(&self) -> &bool32x4 {
        &self.flip_mask
    }

    #[inline]
    fn speculative_margin(&self) -> &f32x4 {
        &self.speculative_margin
    }

    #[inline]
    fn shape_a(&self) -> &SphereWide {
        &self.a
    }

    #[inline]
    fn shape_b(&self) -> &TShapeBWide {
        &self.b
    }

    #[inline]
    fn orientation_a(&self) -> &UnitQuatx4 {
        &UnitQuatx4::IDENTITY
    }

    #[inline]
    fn orientation_b(&self) -> &UnitQuatx4 {
        &self.orientation_b
    }

    #[inline]
    fn offset_b(&self) -> &Vec3x4 {
        &self.offset_b
    }

    #[inline]
    fn set_offset_b(&mut self, value: Vec3x4) {
        self.offset_b = value;
    }

    #[inline]
    fn reduction_id(&self) -> &[ReductionId; 4] {
        &self.reduction_id
    }
}

define_pair_struct!(SphereIncludingPairBatch, SphereIncludingPair, OneShape);

impl_pair_batch!(TShape, SphereIncludingPair, SphereIncludingPairBatch);