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::*;
use glam_det::{Mat3x4, Point3, Point3x4, Vec3, Vec3x4};

use super::BatchesReductionContext;
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::traits::{
    CollisionCallBack, CollisionPairWide, ContactContext, ContactManifoldWide, Orientation,
    OrientationWide, PairWide, PairWideTest, WritePairWide,
};
use crate::{ConvexContactManifold, ShapeContainer};

pub trait CollisionTask<const N: usize> {
    type TPairWide: CollisionPairWide
        + Default
        + PairWide
        + WritePairWide<4>
        + WritePairWide<3>
        + WritePairWide<2>
        + WritePairWide<1>;

    fn need_flip() -> bool;
    #[inline]
    fn flip(pair_wide: &mut Self::TPairWide, manifold_wide: &mut Convex4ContactManifoldWide) {
        let mut offset_b = *pair_wide.offset_b();
        ContactManifoldWide::<ConvexContactManifold>::apply_flip_mask(
            manifold_wide,
            N,
            &mut offset_b,
            pair_wide.flip_mask(),
        );
        pair_wide.set_offset_b(offset_b);
    }

    #[inline]
    fn collision_wide<const PAIR_COUNT: usize>(
        reduction_callback: &mut BatchesReductionContext,
        collision_callback: &mut dyn CollisionCallBack,
        pair_wide: &mut Self::TPairWide,
        manifold_wide: &mut Convex4ContactManifoldWide,
        container: Option<&ShapeContainer>,
    ) where
        ShapeWideTester: PairWideTest<
            <Self::TPairWide as CollisionPairWide>::TShapeWideA,
            <Self::TPairWide as CollisionPairWide>::TShapeWideB,
        >,
    {
        let a_wide = pair_wide.shape_a();
        let b_wide = pair_wide.shape_b();
        let speculative_margin = *pair_wide.speculative_margin();

        let offset_b = pair_wide.offset_b();

        let contact_context = ContactContext {
            orientation_a: pair_wide.orientation_a(),
            orientation_b: pair_wide.orientation_b(),
            offset_b,
            speculative_margin,
            pair_count: PAIR_COUNT,
            complex_shape_container: container,
        };
        let need_reset_manifold = <ShapeWideTester as PairWideTest<
            <Self::TPairWide as CollisionPairWide>::TShapeWideA,
            <Self::TPairWide as CollisionPairWide>::TShapeWideB,
        >>::should_reset_manifold_before_test();
        if need_reset_manifold {
            manifold_wide.reset(N);
        }
        <ShapeWideTester as PairWideTest<
            <Self::TPairWide as CollisionPairWide>::TShapeWideA,
            <Self::TPairWide as CollisionPairWide>::TShapeWideB,
        >>::test(a_wide, b_wide, &contact_context, manifold_wide);

        if Self::need_flip() {
            Self::flip(pair_wide, manifold_wide);
        }
        let mut manifold = ConvexContactManifold::default();

        let reduction_ids = pair_wide.reduction_id();
        let reduction_id_array = reduction_ids.as_ref();

        for (index, reduction_id) in reduction_id_array.iter().enumerate().take(PAIR_COUNT) {
            ContactManifoldWide::<ConvexContactManifold>::get_manifold(
                manifold_wide,
                index,
                N,
                pair_wide.offset_b(),
                &mut manifold,
            );
            reduction_callback.on_pair_complete(*reduction_id, &manifold, collision_callback);
        }
    }

    fn execute_batch(
        pairs: &[<Self::TPairWide as PairWide>::TPair],
        reduction_callback: &mut BatchesReductionContext,
        collision_callback: &mut dyn CollisionCallBack,
        container: Option<&ShapeContainer>,
    ) where
        ShapeWideTester: PairWideTest<
            <Self::TPairWide as CollisionPairWide>::TShapeWideA,
            <Self::TPairWide as CollisionPairWide>::TShapeWideB,
        >,
    {
        let lanes = i32x4::lanes();
        let mut pair_wide = Self::TPairWide::default();
        let mut manifold_wide = Convex4ContactManifoldWide::default();

        let iter = &mut pairs.iter();
        let wide_pair_full_wide_count = pairs.len() / lanes;
        for _ in 0..wide_pair_full_wide_count {
            let pair_0 = iter.next().unwrap();
            let pair_1 = iter.next().unwrap();
            let pair_2 = iter.next().unwrap();
            let pair_3 = iter.next().unwrap();
            pair_wide.write_wide([pair_0, pair_1, pair_2, pair_3]);
            Self::collision_wide::<4>(
                reduction_callback,
                collision_callback,
                &mut pair_wide,
                &mut manifold_wide,
                container,
            );
        }

        match pairs.len() % lanes {
            3 => {
                let mut iter = iter.take(3);
                let pair_0 = iter.next().unwrap();
                let pair_1 = iter.next().unwrap();
                let pair_2 = iter.next().unwrap();
                pair_wide.write_wide([pair_0, pair_1, pair_2]);
                Self::collision_wide::<3>(
                    reduction_callback,
                    collision_callback,
                    &mut pair_wide,
                    &mut manifold_wide,
                    container,
                );
            }
            2 => {
                let mut iter = iter.take(2);
                let pair_0 = iter.next().unwrap();
                let pair_1 = iter.next().unwrap();
                pair_wide.write_wide([pair_0, pair_1]);
                Self::collision_wide::<2>(
                    reduction_callback,
                    collision_callback,
                    &mut pair_wide,
                    &mut manifold_wide,
                    container,
                );
            }
            1 => {
                let mut iter = iter.take(1);
                let pair_0 = iter.next().unwrap();
                pair_wide.write_wide([pair_0]);
                Self::collision_wide::<1>(
                    reduction_callback,
                    collision_callback,
                    &mut pair_wide,
                    &mut manifold_wide,
                    container,
                );
            }
            0 => {}
            _ => unreachable!(),
        }
    }
}

pub trait TransformativeWide {
    type Orientation: OrientationWide;
    fn offset(&self) -> Vec3x4;
    fn orientation(&self) -> &Self::Orientation;

    #[allow(unused)]
    #[inline]
    fn transform_point(&self, point: Point3x4) -> Point3x4 {
        let result = self.orientation().mul_vec3(point.as_vec3x4()) + self.offset();
        Point3x4::from_vec3x4(result)
    }
    #[allow(unused)]
    #[inline]
    fn inverse_transform_point(&self, point: Point3x4) -> Point3x4 {
        let offset = self.orientation().inverse_mul_vec3(-self.offset());
        let point = self.orientation().inverse_mul_vec3(point.as_vec3x4());
        Point3x4::from_vec3x4(offset + point)
    }
}

pub(super) trait Transformative {
    type Orientation: Orientation;
    fn offset(&self) -> Vec3;
    fn orientation(&self) -> &Self::Orientation;

    #[inline]
    fn transform_point(&self, point: Point3) -> Point3 {
        let result = self.orientation().mul_vec3(point.as_vec3()) + self.offset();
        Point3::from_vec3(result)
    }

    #[allow(unused)]
    #[inline]
    fn inverse_transform_point(&self, point: Point3) -> Point3 {
        let offset = self.orientation().inverse_mul_vec3(-self.offset());
        let point = self.orientation().inverse_mul_vec3(point.as_vec3());
        Point3::from_vec3(offset + point)
    }
}

#[derive(Clone, Copy)]
pub enum Axis {
    X = 0,
    Y = 1,
    Z = 2,
}
pub trait AxisInfo {
    fn x_axis(&self) -> Vec3x4;
    fn y_axis(&self) -> Vec3x4;
    fn z_axis(&self) -> Vec3x4;

    #[inline]
    fn axis(&self, index: Axis) -> Vec3x4 {
        match index {
            Axis::X => self.x_axis(),
            Axis::Y => self.y_axis(),
            Axis::Z => self.z_axis(),
        }
    }
}

pub struct TransformWide<'a, T> {
    pub offset: Vec3x4,
    pub orientation: &'a T,
}
impl<'a, T> TransformWide<'a, T> {
    #[inline]
    pub fn new(offset: Vec3x4, orientation: &'a T) -> Self {
        Self {
            offset,
            orientation,
        }
    }
}
impl<'a, T> TransformativeWide for TransformWide<'a, T>
where
    T: OrientationWide,
{
    type Orientation = T;

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

    #[inline]
    fn orientation(&self) -> &Self::Orientation {
        self.orientation
    }
}
pub(crate) struct Transform<'a, T> {
    pub offset: Vec3,
    pub orientation: &'a T,
}
impl<'a, T> Transform<'a, T> {
    #[inline]
    pub fn new(offset: Vec3, orientation: &'a T) -> Self {
        Self {
            offset,
            orientation,
        }
    }
}

impl<'a, T> Transformative for Transform<'a, T>
where
    T: Orientation,
{
    type Orientation = T;

    #[inline]
    fn offset(&self) -> Vec3 {
        self.offset
    }

    #[inline]
    fn orientation(&self) -> &Self::Orientation {
        self.orientation
    }
}

impl<'a> AxisInfo for TransformWide<'a, Mat3x4> {
    #[inline]
    fn x_axis(&self) -> Vec3x4 {
        self.orientation.x_axis
    }

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

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

impl AxisInfo for Mat3x4 {
    #[inline]
    fn x_axis(&self) -> Vec3x4 {
        self.x_axis
    }

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

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