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::nums::Float;
use glam_det::{Dot, UnitQuat, UnitVec3, Vec3};
use smallvec::SmallVec;
use vslab::{new_type_id, Slab};

use super::convex_collision_task::{
    CapsuleTriangleTask, CuboidTriangleTask, SphereTriangleTask, TriangleConvexHullTask,
    TriangleCylinderTask,
};
use super::traits::CollisionTask;
use crate::collision_plugin::{PairGenerateContext, PluginContainer};
use crate::collision_tasks::convex_collision_task::{
    CapsuleConvexHullTask, CapsuleCuboidTask, CapsuleCylinderTask, CapsuleInfinitePlaneTask,
    CapsulePairTask, ConvexHullInfinitePlaneTask, ConvexHullPairTask, CuboidConvexHullTask,
    CuboidCylinderTask, CuboidInfinitePlaneTask, CuboidPairTask, CylinderConvexHullTask,
    CylinderInfinitePlaneTask, CylinderPairTask, SphereCapsuleTask, SphereConvexHullTask,
    SphereCuboidTask, SphereCylinderTask, SphereInfinitePlaneTask, SphereSphereTask,
};
use crate::collision_tasks::pairs::{
    FliplessPair, FliplessPairBatch, SphereIncludingPair, SphereIncludingPairBatch, SpherePair,
    SpherePairBatch, StandardPair, StandardPairBatch,
};
use crate::convex_contact_manifold::OneContact;
use crate::shapes::*;
use crate::traits::{CollisionCallBack, PairBatch};
use crate::{CollisionShapePlugin, ContactManifold, ConvexContactManifold};

#[derive(Clone)]
pub struct Batches {
    shape_batches: ShapeBatches,
    reduction_context: BatchesReductionContext,
    plugin_container: PluginContainer,
}

#[derive(Clone)]
pub struct ShapeBatches {
    pub sphere_pair_batch: SpherePairBatch,
    pub sphere_cuboid_batch: SphereIncludingPairBatch<Cuboid>,
    pub sphere_capsule_batch: SphereIncludingPairBatch<Capsule>,
    pub sphere_infinite_plane_batch: SphereIncludingPairBatch<InfinitePlane>,
    pub sphere_cylinder_batch: SphereIncludingPairBatch<Cylinder>,
    pub capsule_cuboid_batch: StandardPairBatch<Capsule, Cuboid>,
    pub capsule_cylinder_batch: StandardPairBatch<Capsule, Cylinder>,
    pub cuboid_cylinder_batch: StandardPairBatch<Cuboid, Cylinder>,
    pub capsule_infinite_plane_batch: StandardPairBatch<Capsule, InfinitePlane>,
    pub cuboid_infinite_plane_batch: StandardPairBatch<Cuboid, InfinitePlane>,
    pub cylinder_convex_hull_batch: StandardPairBatch<Cylinder, ConvexHullId>,
    pub cuboid_pair_batch: FliplessPairBatch<Cuboid>,
    pub capsule_pair_batch: FliplessPairBatch<Capsule>,
    pub cylinder_pair_batch: FliplessPairBatch<Cylinder>,
    pub cylinder_infinite_plane_batch: StandardPairBatch<Cylinder, InfinitePlane>,
    pub convex_hull_infinite_plane_batch: StandardPairBatch<ConvexHullId, InfinitePlane>,
    pub convex_hull_pair_batch: FliplessPairBatch<ConvexHullId>,
    pub sphere_convex_hull_batch: SphereIncludingPairBatch<ConvexHullId>,
    pub capsule_convex_hull_batch: StandardPairBatch<Capsule, ConvexHullId>,
    pub cuboid_convex_hull_batch: StandardPairBatch<Cuboid, ConvexHullId>,

    pub sphere_triangle_batch: SphereIncludingPairBatch<Triangle>,
    pub capsule_triangle_batch: StandardPairBatch<Capsule, Triangle>,
    pub cuboid_triangle_batch: StandardPairBatch<Cuboid, Triangle>,
    pub triangle_cylinder_batch: StandardPairBatch<Triangle, Cylinder>,
    pub triangle_convex_hull_batch: StandardPairBatch<Triangle, ConvexHullId>,
}
impl ShapeBatches {
    fn clear(&mut self) {
        self.sphere_pair_batch.clear();
        self.sphere_cuboid_batch.clear();
        self.sphere_capsule_batch.clear();
        self.sphere_infinite_plane_batch.clear();
        self.sphere_cylinder_batch.clear();
        self.capsule_cuboid_batch.clear();
        self.capsule_cylinder_batch.clear();
        self.cuboid_cylinder_batch.clear();
        self.capsule_infinite_plane_batch.clear();
        self.cuboid_infinite_plane_batch.clear();
        self.cylinder_infinite_plane_batch.clear();
        self.convex_hull_infinite_plane_batch.clear();
        self.cuboid_pair_batch.clear();
        self.capsule_pair_batch.clear();
        self.cylinder_pair_batch.clear();
        self.convex_hull_pair_batch.clear();
        self.capsule_convex_hull_batch.clear();
        self.sphere_convex_hull_batch.clear();
        self.cylinder_convex_hull_batch.clear();
        self.cuboid_convex_hull_batch.clear();
        self.sphere_triangle_batch.clear();
        self.capsule_triangle_batch.clear();
        self.cuboid_triangle_batch.clear();
        self.triangle_cylinder_batch.clear();
        self.triangle_convex_hull_batch.clear();
    }

    fn new() -> Self {
        Self {
            sphere_pair_batch: SpherePairBatch::new(4),
            sphere_cuboid_batch: SphereIncludingPairBatch::new(4),
            sphere_capsule_batch: SphereIncludingPairBatch::new(4),
            sphere_infinite_plane_batch: SphereIncludingPairBatch::new(4),
            sphere_cylinder_batch: SphereIncludingPairBatch::new(4),
            capsule_cuboid_batch: StandardPairBatch::new(4),
            capsule_cylinder_batch: StandardPairBatch::new(4),
            cuboid_cylinder_batch: StandardPairBatch::new(4),
            capsule_infinite_plane_batch: StandardPairBatch::new(4),
            cuboid_infinite_plane_batch: StandardPairBatch::new(4),
            cylinder_infinite_plane_batch: StandardPairBatch::new(4),
            convex_hull_infinite_plane_batch: StandardPairBatch::new(4),
            cylinder_convex_hull_batch: StandardPairBatch::new(4),
            cuboid_pair_batch: FliplessPairBatch::new(4),
            capsule_pair_batch: FliplessPairBatch::new(4),
            cylinder_pair_batch: FliplessPairBatch::new(4),
            convex_hull_pair_batch: FliplessPairBatch::new(4),
            capsule_convex_hull_batch: StandardPairBatch::new(4),
            sphere_convex_hull_batch: SphereIncludingPairBatch::new(4),
            cuboid_convex_hull_batch: StandardPairBatch::new(4),
            sphere_triangle_batch: SphereIncludingPairBatch::new(4),
            capsule_triangle_batch: StandardPairBatch::new(4),
            cuboid_triangle_batch: StandardPairBatch::new(4),
            triangle_cylinder_batch: StandardPairBatch::new(4),
            triangle_convex_hull_batch: StandardPairBatch::new(4),
        }
    }

    pub fn push_sphere_triangle(&mut self, pair: SphereIncludingPair<Triangle>) {
        self.sphere_triangle_batch.push(pair);
    }

    pub fn push_capsule_triangle(&mut self, pair: StandardPair<Capsule, Triangle>) {
        self.capsule_triangle_batch.push(pair);
    }

    pub fn push_cuboid_triangle(&mut self, pair: StandardPair<Cuboid, Triangle>) {
        self.cuboid_triangle_batch.push(pair);
    }

    pub fn push_triangle_cylinder(&mut self, pair: StandardPair<Triangle, Cylinder>) {
        self.triangle_cylinder_batch.push(pair);
    }

    pub fn push_triangle_convex_hull(&mut self, pair: StandardPair<Triangle, ConvexHullId>) {
        self.triangle_convex_hull_batch.push(pair);
    }
}

new_type_id!(ReductionId);
new_type_id!(ConcaveReductionId);

const MAX_MANIFOLD: usize = 16;

#[derive(Clone, Copy)]
enum ReductionType {
    None,
    Concave,
}
#[derive(Default, Clone)]
struct ConcaveReductionContext {
    count: usize,
    call_back_count: usize,
    manifolds: SmallVec<[OneContact; MAX_MANIFOLD]>,
}

#[derive(Clone, Copy)]
struct IndexScore {
    index: usize,
    score: f32,
}

impl Default for IndexScore {
    fn default() -> Self {
        Self {
            index: usize::MAX,
            score: f32::MIN,
        }
    }
}

impl ConcaveReductionContext {
    #[inline]
    fn clear(&mut self) {
        self.count = 0;
        self.call_back_count = 0;
        self.manifolds.clear();
    }

    #[inline]
    fn push(
        &mut self,
        pair_id: usize,
        manifold: &impl ContactManifold,
        call_back: &mut dyn CollisionCallBack,
    ) {
        self.call_back_count += 1;

        for i in 0..manifold.contact_count() {
            let one_contact = OneContact {
                offset_b: manifold.offset_b(),
                normal: manifold.get_contact_normal(i),
                offset_a: manifold.get_contact_offset_a(i),
                depth: manifold.get_contact_depth(i),
                feature_id: manifold.get_contact_feature_id(i),
            };
            self.manifolds.push(one_contact);
        }
        if self.count == self.call_back_count {
            let concave_manifold = self.reduce();

            if concave_manifold.count > 0 {
                call_back.on_pair_complete(
                    pair_id,
                    crate::convex_contact_manifold::ManifoldRef::Concave(&concave_manifold),
                );
            }
            self.clear();
        }
    }

    // choose 4 most reasonable manifolds
    fn reduce(&mut self) -> ConcaveManifold {
        // new reduction algorithm here
        let mut result = ConcaveManifold::default();
        if self.manifolds.is_empty() {
            return result;
        }
        let mut candidate_manifolds = SmallVec::<[IndexScore; MAX_MANIFOLD]>::new();
        let (best_index, max_distance_square, max_distance) =
            self.product_candidate_manifolds_and_choose_max_score(&mut candidate_manifolds);

        if best_index == usize::MAX {
            return result;
        }
        result.manifolds[0] = self.manifolds[best_index].clone();
        result.count = 1;
        // all the contact point is same
        if max_distance < 1e-6 {
            return result;
        }

        while result.count < ConcaveManifold::MAX_CONTACTS && !candidate_manifolds.is_empty() {
            let last_manifold = &result.manifolds[result.count - 1];
            let mut best_score_index = IndexScore::default();
            let mut i = 0;
            let mut best_index = usize::MAX;
            while i < candidate_manifolds.len() {
                let manifold = &mut candidate_manifolds[i];
                let index = manifold.index;
                let diff = diff_score(
                    last_manifold,
                    &self.manifolds[index],
                    max_distance_square,
                    1e-5,
                    400.0_f32 * max_distance.recip(),
                );
                if diff <= 1e-6 {
                    candidate_manifolds.remove(i);
                    continue;
                }
                manifold.score = manifold.score.minf(diff);
                if manifold.score > best_score_index.score {
                    best_score_index = *manifold;
                    best_index = i;
                }
                i += 1;
            }

            if best_score_index.index != usize::MAX {
                result.manifolds[result.count] = self.manifolds[best_score_index.index].clone();
                result.count += 1;
                candidate_manifolds.remove(best_index);
            }
        }
        assert!(result.count <= ConcaveManifold::MAX_CONTACTS);
        assert!(result.count > 0);
        result
    }

    fn product_candidate_manifolds_and_choose_max_score(
        &mut self,
        candidate_manifolds: &mut SmallVec<[IndexScore; MAX_MANIFOLD]>,
    ) -> (usize, f32, f32) {
        let random_extern = Vec3::new(0.8991, 0.4103, 0.1526);
        let (min_extern, min_extern_pos) = self.manifolds.iter().fold(
            (f32::MAX, Vec3::ZERO),
            |(min_extern, min_extern_pos), one_contact| {
                let pos = one_contact.offset_a;
                let extern_score = random_extern.dot(pos);
                if extern_score < min_extern {
                    (extern_score, pos)
                } else {
                    (min_extern, min_extern_pos)
                }
            },
        );

        let max_distance_square = self
            .manifolds
            .iter()
            .fold(0.0, |max_distance, one_contact| {
                let pos = one_contact.offset_a;
                let distance = (pos - min_extern_pos).length_squared();
                distance.max(max_distance)
            });
        let max_distance = max_distance_square.sqrtf();
        let scale = max_distance * 0.001;

        let best_index_score = self.manifolds.iter().enumerate().fold(
            IndexScore::default(),
            |best_index_score, (i, one_contact)| {
                let pos = one_contact.offset_a;
                let extern_score = random_extern.dot(pos) - min_extern;
                let score = if one_contact.depth > 0.0 {
                    extern_score * scale + one_contact.depth
                } else {
                    one_contact.depth
                };
                let index_score = IndexScore { index: i, score };
                candidate_manifolds.push(index_score);
                if score > best_index_score.score {
                    index_score
                } else {
                    best_index_score
                }
            },
        );
        if best_index_score.index != usize::MAX {
            candidate_manifolds.remove(best_index_score.index);
        }
        (best_index_score.index, max_distance_square, max_distance)
    }
}

fn diff_score(
    origin: &OneContact,
    other: &OneContact,
    max_len_square: f32,
    tolerance: f32,
    depth_scale: f32,
) -> f32 {
    // diff_max = max(offset_score, normal_score)
    // diff_combined = offset_score * normal_score
    // diff0= min(diff_max, diff_combined)
    // diff1 = clamp(diff0, 0.0, diff0)
    // diff_score = diff1 * score_scale,
    // score_scale = 1+ depth * depth_scale
    let offset_score = offset_score(origin.offset_a, other.offset_a, max_len_square);
    let normal_score = normal_score(origin.normal.as_vec3(), other.normal.as_vec3(), tolerance);
    let diff_max = offset_score.max(normal_score);
    let diff_combined = offset_score * normal_score;
    let diff0 = diff_max.min(diff_combined);
    if diff0 < 0.0 {
        return 0.0;
    }
    diff0 * score_scale(other.depth, depth_scale)
}
fn offset_score(offset0: Vec3, offset1: Vec3, max_len_square: f32) -> f32 {
    let diff = offset0 - offset1;
    let diff_len_square = diff.length_squared();
    let score = diff_len_square - (max_len_square * 1e-6);
    let max_len_square_recip = max_len_square.recip();
    score / max_len_square_recip
}
/// d = dot(normal, normal0)
/// `normal_score` = (d+(t-1))/(t-1)
/// `normal_score` -> [-1e-5,2+1e-5]
fn normal_score(normal0: Vec3, normal1: Vec3, t: f32) -> f32 {
    let d = normal0.dot(normal1);
    let t_minus_1 = t - 1.0;
    (d + t_minus_1) * t_minus_1.recip()
}

fn score_scale(depth: f32, depth_scale: f32) -> f32 {
    1.0 + depth_scale * depth
}

pub struct ConcaveManifold {
    count: usize,
    manifolds: [OneContact; 4],
}
impl Default for ConcaveManifold {
    fn default() -> Self {
        let manifold = OneContact::default();
        Self {
            count: 0,
            manifolds: [
                manifold.clone(),
                manifold.clone(),
                manifold.clone(),
                manifold,
            ],
        }
    }
}

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

    fn offset_b(&self) -> Vec3 {
        unreachable!("ConcaveManifold does not have offset_b")
    }

    fn contact_count(&self) -> usize {
        self.count
    }

    fn get_contact_normal(&self, contact_index: usize) -> UnitVec3 {
        self.manifolds[contact_index].normal
    }

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

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

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

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

impl ConcaveReductionContext {}

#[derive(Clone)]
struct ReductionContext {
    pub pair_id: usize,
    pub reduction_type: ReductionType,
    pub child_reduction_id: ConcaveReductionId,
}

impl ReductionContext {
    #[must_use]
    #[inline]
    fn new(pair_id: usize) -> Self {
        ReductionContext {
            pair_id,
            reduction_type: ReductionType::None,
            child_reduction_id: ConcaveReductionId::invalid(),
        }
    }
}

#[derive(Clone)]
pub struct BatchesReductionContext {
    reduction_contexts: Slab<ReductionId, ReductionContext>,
    concave_reduction_contexts: Slab<ConcaveReductionId, ConcaveReductionContext>,
}
const CONCAVE_REDUCTION_CAPACITY: usize = 1024;
const REDUCTION_CAPACITY: usize = 1024;
impl BatchesReductionContext {
    fn new() -> Self {
        BatchesReductionContext {
            reduction_contexts: Slab::with_capacity(REDUCTION_CAPACITY),
            concave_reduction_contexts: Slab::with_capacity(CONCAVE_REDUCTION_CAPACITY),
        }
    }

    fn clear(&mut self) {
        self.reduction_contexts.clear();
        self.concave_reduction_contexts.clear();
    }

    pub fn on_pair_complete(
        &mut self,
        reduction_id: ReductionId,
        manifold: &ConvexContactManifold,
        call_back: &mut dyn CollisionCallBack,
    ) {
        //safe unwrap
        let reduction_context = self.reduction_contexts.get_mut(reduction_id).unwrap();

        match reduction_context.reduction_type {
            ReductionType::None => {
                if manifold.contact_count() > 0 {
                    call_back.on_pair_complete(
                        reduction_context.pair_id,
                        crate::convex_contact_manifold::ManifoldRef::Convex(manifold),
                    );
                }
            }
            ReductionType::Concave => {
                let concave_reduction_id = reduction_context.child_reduction_id;
                let concave_reduction_context = self
                    .concave_reduction_contexts
                    .get_mut(concave_reduction_id)
                    .unwrap();
                concave_reduction_context.push(reduction_context.pair_id, manifold, call_back);
            }
        }
    }

    pub fn create_reduction_pair(&mut self, pair_id: usize) -> ReductionId {
        let reduction = ReductionContext::new(pair_id);
        self.reduction_contexts.insert(reduction)
    }

    pub fn record_reduction(&mut self, reduction_id: ReductionId, count: usize) {
        let reduction_context = self.reduction_contexts.get_mut(reduction_id).unwrap();
        let concave_reduction_id = reduction_context.child_reduction_id;
        self.concave_reduction_contexts
            .get_mut(concave_reduction_id)
            .unwrap()
            .count = count;
    }

    #[inline]
    pub fn set_to_concave_reduction(&mut self, reduction_id: ReductionId) {
        //safe unwrap
        let reduction_context = self.reduction_contexts.get_mut(reduction_id).unwrap();
        reduction_context.reduction_type = ReductionType::Concave;
        reduction_context.child_reduction_id = self
            .concave_reduction_contexts
            .insert(ConcaveReductionContext::default());
    }
}
impl Batches {
    pub fn register_plugin(&mut self, plugin: impl CollisionShapePlugin) {
        self.plugin_container.add(plugin);
    }
}

pub struct ContactContext<'a> {
    pub a: &'a Shape,
    pub b: &'a Shape,
    pub offset_b: &'a Vec3,
    pub orientation_a: &'a UnitQuat,
    pub orientation_b: &'a UnitQuat,
}

impl ContactContext<'_> {
    #[must_use]
    pub fn new<'a>(
        a: &'a Shape,
        b: &'a Shape,
        offset_b: &'a Vec3,
        orientation_a: &'a UnitQuat,
        orientation_b: &'a UnitQuat,
    ) -> ContactContext<'a> {
        ContactContext {
            a,
            b,
            offset_b,
            orientation_a,
            orientation_b,
        }
    }
}

impl Batches {
    pub fn clear(&mut self) {
        self.shape_batches.clear();
        self.reduction_context.clear();
        self.plugin_container.clear();
    }

    pub fn execute(&mut self, container: &ShapeContainer, callback: &mut dyn CollisionCallBack) {
        let container = Some(container);
        //let callback = &mut Box::new(callback);
        SphereSphereTask::execute_batch(
            self.shape_batches.sphere_pair_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        SphereCuboidTask::execute_batch(
            self.shape_batches.sphere_cuboid_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        SphereCapsuleTask::execute_batch(
            self.shape_batches.sphere_capsule_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        SphereInfinitePlaneTask::execute_batch(
            self.shape_batches.sphere_infinite_plane_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        SphereCylinderTask::execute_batch(
            self.shape_batches.sphere_cylinder_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CapsuleCuboidTask::execute_batch(
            self.shape_batches.capsule_cuboid_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CapsuleCylinderTask::execute_batch(
            self.shape_batches.capsule_cylinder_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CuboidCylinderTask::execute_batch(
            self.shape_batches.cuboid_cylinder_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CapsuleInfinitePlaneTask::execute_batch(
            self.shape_batches.capsule_infinite_plane_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CuboidInfinitePlaneTask::execute_batch(
            self.shape_batches.cuboid_infinite_plane_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CylinderInfinitePlaneTask::execute_batch(
            self.shape_batches.cylinder_infinite_plane_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        ConvexHullInfinitePlaneTask::execute_batch(
            self.shape_batches
                .convex_hull_infinite_plane_batch
                .get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CylinderConvexHullTask::execute_batch(
            self.shape_batches.cylinder_convex_hull_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CuboidPairTask::execute_batch(
            self.shape_batches.cuboid_pair_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CapsulePairTask::execute_batch(
            self.shape_batches.capsule_pair_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CylinderPairTask::execute_batch(
            self.shape_batches.cylinder_pair_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        ConvexHullPairTask::execute_batch(
            self.shape_batches.convex_hull_pair_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CapsuleConvexHullTask::execute_batch(
            self.shape_batches.capsule_convex_hull_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        SphereConvexHullTask::execute_batch(
            self.shape_batches.sphere_convex_hull_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );
        CuboidConvexHullTask::execute_batch(
            self.shape_batches.cuboid_convex_hull_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );

        SphereTriangleTask::execute_batch(
            self.shape_batches.sphere_triangle_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );

        CapsuleTriangleTask::execute_batch(
            self.shape_batches.capsule_triangle_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );

        CuboidTriangleTask::execute_batch(
            self.shape_batches.cuboid_triangle_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );

        TriangleCylinderTask::execute_batch(
            self.shape_batches.triangle_cylinder_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );

        TriangleConvexHullTask::execute_batch(
            self.shape_batches.triangle_convex_hull_batch.get_pairs(),
            &mut self.reduction_context,
            callback,
            container,
        );

        self.plugin_container
            .execute(container, &mut self.reduction_context, callback);
    }

    #[inline]
    #[cfg(test)]
    #[allow(clippy::too_many_arguments)]
    pub(crate) fn push_test(
        &mut self,
        pair_id: usize,
        a: &Shape,
        b: &Shape,
        offset_b: Vec3,
        orientation_a: UnitQuat,
        orientation_b: UnitQuat,
        shapes: &ShapeContainer,
        speculative_margin: f32,
    ) {
        self.push(
            pair_id,
            &ContactContext {
                a,
                b,
                offset_b: &offset_b,
                orientation_a: &orientation_a,
                orientation_b: &orientation_b,
            },
            shapes,
            speculative_margin,
        );
    }

    /// Push a pair of shapes for collision detection.
    ///
    /// # Panics
    ///
    /// Panics if either shape is `Shape::Empty`.
    pub fn push(
        &mut self,
        pair_id: usize,
        context: &ContactContext,
        shapes: &ShapeContainer,
        speculative_margin: f32,
    ) {
        let a = context.a;
        let b = context.b;
        let offset_b = *context.offset_b;
        let orientation_a = *context.orientation_a;
        let orientation_b = *context.orientation_b;
        let reduction = ReductionContext::new(pair_id);
        let reduction_id = self.reduction_context.reduction_contexts.insert(reduction);
        match (a, b) {
            (Shape::Cuboid(cuboid_a), Shape::Cuboid(cuboid_b)) => {
                self.shape_batches.cuboid_pair_batch.push(FliplessPair {
                    a: *cuboid_a,
                    b: *cuboid_b,
                    offset_b,
                    orientation_a,
                    orientation_b,
                    speculative_margin,
                    reduction_id,
                });
            }
            (Shape::Cuboid(cuboid_a), Shape::Sphere(sphere_b)) => {
                self.shape_batches
                    .sphere_cuboid_batch
                    .push(SphereIncludingPair {
                        a: *sphere_b,
                        b: *cuboid_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Cuboid(cuboid_a), Shape::Capsule(capsule_b)) => {
                self.shape_batches.capsule_cuboid_batch.push(StandardPair {
                    a: *capsule_b,
                    b: *cuboid_a,
                    flip_mask: true,
                    offset_b: -offset_b,
                    orientation_a: orientation_b,
                    orientation_b: orientation_a,
                    speculative_margin,
                    reduction_id,
                });
            }
            (Shape::Sphere(sphere_a), Shape::Cuboid(cuboid_b)) => {
                self.shape_batches
                    .sphere_cuboid_batch
                    .push(SphereIncludingPair {
                        a: *sphere_a,
                        b: *cuboid_b,
                        flip_mask: false,
                        offset_b,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Sphere(sphere_a), Shape::Sphere(sphere_b)) => {
                self.shape_batches.sphere_pair_batch.push(SpherePair {
                    shape_a: *sphere_a,
                    shape_b: *sphere_b,
                    offset_b,
                    speculative_margin,
                    reduction_id,
                });
            }
            (Shape::Sphere(sphere_a), Shape::Capsule(capsule_b)) => {
                self.shape_batches
                    .sphere_capsule_batch
                    .push(SphereIncludingPair {
                        a: *sphere_a,
                        b: *capsule_b,
                        flip_mask: false,
                        offset_b,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Capsule(capsule_a), Shape::Cuboid(cuboid_b)) => {
                self.shape_batches.capsule_cuboid_batch.push(StandardPair {
                    a: *capsule_a,
                    b: *cuboid_b,
                    flip_mask: false,
                    offset_b,
                    orientation_a,
                    orientation_b,
                    speculative_margin,
                    reduction_id,
                });
            }
            (Shape::Capsule(capsule_a), Shape::Sphere(sphere_b)) => {
                self.shape_batches
                    .sphere_capsule_batch
                    .push(SphereIncludingPair {
                        a: *sphere_b,
                        b: *capsule_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Capsule(capsule_a), Shape::Capsule(capsule_b)) => {
                self.shape_batches.capsule_pair_batch.push(FliplessPair {
                    a: *capsule_a,
                    b: *capsule_b,
                    offset_b,
                    orientation_a,
                    orientation_b,
                    speculative_margin,
                    reduction_id,
                });
            }

            (Shape::Sphere(sphere_a), Shape::InfinitePlane(infinite_plane_b)) => {
                self.shape_batches
                    .sphere_infinite_plane_batch
                    .push(SphereIncludingPair {
                        a: *sphere_a,
                        b: *infinite_plane_b,
                        flip_mask: false,
                        offset_b,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::InfinitePlane(infinite_plane_a), Shape::Sphere(sphere_b)) => {
                self.shape_batches
                    .sphere_infinite_plane_batch
                    .push(SphereIncludingPair {
                        a: *sphere_b,
                        b: *infinite_plane_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Cylinder(cylinder_a), Shape::InfinitePlane(infinite_plane_b)) => {
                self.shape_batches
                    .cylinder_infinite_plane_batch
                    .push(StandardPair {
                        a: *cylinder_a,
                        b: *infinite_plane_b,
                        flip_mask: false,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::InfinitePlane(infinite_plane_a), Shape::Cylinder(cylinder_b)) => {
                self.shape_batches
                    .cylinder_infinite_plane_batch
                    .push(StandardPair {
                        a: *cylinder_b,
                        b: *infinite_plane_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_a: orientation_b,
                        orientation_b: orientation_a,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Cylinder(cylinder_a), Shape::Cylinder(cylinder_b)) => {
                self.shape_batches.cylinder_pair_batch.push(FliplessPair {
                    a: *cylinder_a,
                    b: *cylinder_b,
                    offset_b,
                    orientation_a,
                    orientation_b,
                    speculative_margin,
                    reduction_id,
                });
            }
            (Shape::Cylinder(cylinder_a), Shape::Sphere(sphere_b)) => {
                self.shape_batches
                    .sphere_cylinder_batch
                    .push(SphereIncludingPair {
                        a: *sphere_b,
                        b: *cylinder_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Sphere(sphere_a), Shape::Cylinder(cylinder_b)) => {
                self.shape_batches
                    .sphere_cylinder_batch
                    .push(SphereIncludingPair {
                        a: *sphere_a,
                        b: *cylinder_b,
                        flip_mask: false,
                        offset_b,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Capsule(capsule_a), Shape::Cylinder(cylinder_b)) => {
                self.shape_batches
                    .capsule_cylinder_batch
                    .push(StandardPair {
                        a: *capsule_a,
                        b: *cylinder_b,
                        flip_mask: false,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Cylinder(cylinder_a), Shape::Capsule(capsule_b)) => {
                self.shape_batches
                    .capsule_cylinder_batch
                    .push(StandardPair {
                        a: *capsule_b,
                        b: *cylinder_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        orientation_a: orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Cuboid(cuboid_a), Shape::Cylinder(cylinder_b)) => {
                self.shape_batches.cuboid_cylinder_batch.push(StandardPair {
                    a: *cuboid_a,
                    b: *cylinder_b,
                    flip_mask: false,
                    offset_b,
                    orientation_a,
                    orientation_b,
                    speculative_margin,
                    reduction_id,
                });
            }
            (Shape::Cylinder(cylinder_a), Shape::Cuboid(cuboid_b)) => {
                self.shape_batches.cuboid_cylinder_batch.push(StandardPair {
                    a: *cuboid_b,
                    b: *cylinder_a,
                    flip_mask: true,
                    offset_b: -offset_b,
                    orientation_b: orientation_a,
                    orientation_a: orientation_b,
                    speculative_margin,
                    reduction_id,
                });
            }
            (Shape::Capsule(capsule_a), Shape::InfinitePlane(infinite_plane_b)) => {
                self.shape_batches
                    .capsule_infinite_plane_batch
                    .push(StandardPair {
                        a: *capsule_a,
                        b: *infinite_plane_b,
                        flip_mask: false,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::InfinitePlane(infinite_plane_a), Shape::Capsule(capsule_b)) => {
                self.shape_batches
                    .capsule_infinite_plane_batch
                    .push(StandardPair {
                        a: *capsule_b,
                        b: *infinite_plane_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_a: orientation_b,
                        orientation_b: orientation_a,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::ConvexHull(convex_hull_a), Shape::InfinitePlane(infinite_plane_b)) => {
                self.shape_batches
                    .convex_hull_infinite_plane_batch
                    .push(StandardPair {
                        a: *convex_hull_a,
                        b: *infinite_plane_b,
                        flip_mask: false,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::InfinitePlane(infinite_plane_a), Shape::ConvexHull(convex_hull_b)) => {
                self.shape_batches
                    .convex_hull_infinite_plane_batch
                    .push(StandardPair {
                        a: *convex_hull_b,
                        b: *infinite_plane_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_a: orientation_b,
                        orientation_b: orientation_a,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::ConvexHull(a), Shape::ConvexHull(b)) => {
                self.shape_batches
                    .convex_hull_pair_batch
                    .push(FliplessPair {
                        a: *a,
                        b: *b,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Cuboid(cuboid_a), Shape::InfinitePlane(infinite_plane_b)) => {
                self.shape_batches
                    .cuboid_infinite_plane_batch
                    .push(StandardPair {
                        a: *cuboid_a,
                        b: *infinite_plane_b,
                        flip_mask: false,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::InfinitePlane(infinite_plane_a), Shape::Cuboid(cuboid_b)) => {
                self.shape_batches
                    .cuboid_infinite_plane_batch
                    .push(StandardPair {
                        a: *cuboid_b,
                        b: *infinite_plane_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        orientation_a: orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Sphere(sphere_a), Shape::ConvexHull(id_b)) => {
                self.shape_batches
                    .sphere_convex_hull_batch
                    .push(SphereIncludingPair {
                        a: *sphere_a,
                        b: *id_b,
                        flip_mask: false,
                        offset_b,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::ConvexHull(id_a), Shape::Sphere(sphere_b)) => {
                self.shape_batches
                    .sphere_convex_hull_batch
                    .push(SphereIncludingPair {
                        a: *sphere_b,
                        b: *id_a,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Capsule(capsule), Shape::ConvexHull(id)) => {
                self.shape_batches
                    .capsule_convex_hull_batch
                    .push(StandardPair {
                        a: *capsule,
                        b: *id,
                        flip_mask: false,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::ConvexHull(id), Shape::Capsule(capsule)) => {
                self.shape_batches
                    .capsule_convex_hull_batch
                    .push(StandardPair {
                        a: *capsule,
                        b: *id,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        orientation_a: orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Cylinder(cylinder), Shape::ConvexHull(id)) => {
                self.shape_batches
                    .cylinder_convex_hull_batch
                    .push(StandardPair {
                        a: *cylinder,
                        b: *id,
                        flip_mask: false,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::ConvexHull(id), Shape::Cylinder(cylinder)) => {
                self.shape_batches
                    .cylinder_convex_hull_batch
                    .push(StandardPair {
                        a: *cylinder,
                        b: *id,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        orientation_a: orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::Cuboid(cuboid), Shape::ConvexHull(id)) => {
                self.shape_batches
                    .cuboid_convex_hull_batch
                    .push(StandardPair {
                        a: *cuboid,
                        b: *id,
                        flip_mask: false,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            (Shape::ConvexHull(id), Shape::Cuboid(cuboid)) => {
                self.shape_batches
                    .cuboid_convex_hull_batch
                    .push(StandardPair {
                        a: *cuboid,
                        b: *id,
                        flip_mask: true,
                        offset_b: -offset_b,
                        orientation_b: orientation_a,
                        orientation_a: orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
            }
            // flip_mask is false means custom shape is ShapeB.
            (
                _,
                Shape::Custom {
                    plugin_id,
                    complex_shape_id,
                },
            ) => {
                self.plugin_container
                    .get_plugin_mut(*plugin_id)
                    .expect("plugin not found")
                    .on_pair_generate(
                        &mut PairGenerateContext {
                            pair_id,
                            shape_batches: &mut self.shape_batches,
                            reduction_context: &mut self.reduction_context,
                            shape: context.a,
                            complex_shape_id,
                            speculative_margin,
                            flip_mask: false,
                        },
                        context,
                        shapes,
                    );
            }
            (
                Shape::Custom {
                    plugin_id,
                    complex_shape_id,
                },
                _,
            ) => {
                self.plugin_container
                    .get_plugin_mut(*plugin_id)
                    .expect("plugin not found")
                    .on_pair_generate(
                        &mut PairGenerateContext {
                            pair_id,
                            shape_batches: &mut self.shape_batches,
                            reduction_context: &mut self.reduction_context,
                            shape: context.b,
                            complex_shape_id,
                            speculative_margin,
                            flip_mask: true,
                        },
                        context,
                        shapes,
                    );
            }
            _ => {
                todo!("issue #1200")
            }
        }
    }
}

impl Default for Batches {
    fn default() -> Self {
        Self {
            shape_batches: ShapeBatches::new(),
            reduction_context: BatchesReductionContext::new(),
            plugin_container: PluginContainer::default(),
        }
    }
}

#[cfg(test)]
mod tests {

    use approx_det::assert_relative_eq;
    use glam::{vec3, Point3};
    use glam_det::{Isometry3, UnitVec3};
    use phys_geom::ComputeAabb3;
    use wasm_bindgen_test::wasm_bindgen_test;

    use super::*;
    use crate::convex_contact_manifold::{ConvexContact, ManifoldRef};
    use crate::ray::{Raycast, RaycastHitResult};
    use crate::shapes::CuboidExt;
    use crate::traits::MinkowskiSupport;
    use crate::{
        collision_plugin, ContactManifold, ContainsPoint, ConvexContactManifold, Expansion,
        SignedDistanceToPoint,
    };

    #[derive(Default)]
    struct CallbackTest {
        task_nums: usize,
        convex_results: Vec<ConvexContactManifold>,
        pairs: Vec<(Isometry3, Isometry3)>,
        call_back_count: usize,
    }

    impl CallbackTest {
        fn new(task_nums: usize) -> Self {
            let mut convex_results = Vec::with_capacity(task_nums);
            convex_results.resize_with(task_nums, Default::default);
            Self {
                task_nums,
                convex_results,
                pairs: Vec::with_capacity(task_nums),
                call_back_count: 0,
            }
        }

        fn resize(&mut self, task_nums: usize) {
            self.task_nums = task_nums;
            self.convex_results.resize_with(task_nums, Default::default);
            self.call_back_count = 0;
        }
    }
    impl CollisionCallBack for CallbackTest {
        #[allow(clippy::field_reassign_with_default)]
        fn on_pair_complete(&mut self, pair_id: usize, manifold: ManifoldRef) {
            match manifold {
                ManifoldRef::Convex(manifold) => {
                    let mut convex_manifold = ConvexContactManifold::default();
                    convex_manifold.count = manifold.contact_count();
                    convex_manifold.offset_b = manifold.offset_b();
                    convex_manifold.normal = manifold.get_contact_normal(0);

                    for index in 0..manifold.contact_count() {
                        convex_manifold.contacts[index].depth = manifold.get_contact_depth(index);
                        convex_manifold.contacts[index].feature_id =
                            manifold.get_contact_feature_id(index);
                        convex_manifold.contacts[index].offset_a =
                            manifold.get_contact_offset_a(index);
                    }

                    self.convex_results[pair_id] = convex_manifold;
                    self.call_back_count += 1;
                }
                ManifoldRef::Concave(_) => {}
            }
        }
    }

    fn check_manifold(a: &ConvexContactManifold, b: &ConvexContactManifold) {
        assert_relative_eq!(a.offset_b, b.offset_b);
        assert_relative_eq!(a.normal, b.normal);
        assert_eq!(a.count, b.count);
        for i in 0..4 {
            let contact_a = &a.contacts[i];
            let contact_b = &b.contacts[i];
            assert_relative_eq!(contact_a.depth, contact_b.depth);
            assert_relative_eq!(contact_a.offset_a, contact_b.offset_a);
            assert_eq!(contact_a.feature_id, contact_b.feature_id);
        }
    }

    #[test]
    #[wasm_bindgen_test]
    fn test_batches_push_cylinder() {
        let _ = env_logger::builder().is_test(true).try_init();

        let cylinder_1 = Cylinder::new(1.0, 0.5);
        let cylinder_2 = Cylinder::new(2.0, 3.0);
        let cuboid = Cuboid::new(Vec3::new(1.0, 2.0, 3.0));
        let sphere = Sphere::new(1.0);
        let capsule = Capsule::new(1.0, 0.5);
        let offset_b = Vec3::new(1.0, 2.0, 3.0);
        let orientation_a = UnitQuat::from_axis_angle(UnitVec3::X, std::f32::consts::PI);
        let orientation_b = UnitQuat::from_axis_angle(UnitVec3::Y, std::f32::consts::PI);

        let mut batches = Batches::default();
        let container = ShapeContainer::default();

        let mut pair_id = 0_usize;

        batches.push_test(
            pair_id,
            &Shape::Cylinder(cylinder_1),
            &Shape::Cylinder(cylinder_2),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Cuboid(cuboid),
            &Shape::Cylinder(cylinder_1),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0,
        );
        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Cylinder(cylinder_1),
            &Shape::Cuboid(cuboid),
            -offset_b,
            orientation_b,
            orientation_a,
            &container,
            0.0,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Sphere(sphere),
            &Shape::Cylinder(cylinder_1),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0,
        );
        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Cylinder(cylinder_1),
            &Shape::Sphere(sphere),
            -offset_b,
            orientation_b,
            orientation_a,
            &container,
            0.0,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Capsule(capsule),
            &Shape::Cylinder(cylinder_1),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0,
        );
        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Cylinder(cylinder_1),
            &Shape::Capsule(capsule),
            -offset_b,
            orientation_b,
            orientation_a,
            &container,
            0.0,
        );

        pair_id += 1;
        assert!(batches.shape_batches.cylinder_pair_batch.get_pairs().len() == 1);
        assert!(
            batches
                .shape_batches
                .cuboid_cylinder_batch
                .get_pairs()
                .len()
                == 2
        );
        assert!(
            batches
                .shape_batches
                .sphere_cylinder_batch
                .get_pairs()
                .len()
                == 2
        );
        assert!(
            batches
                .shape_batches
                .capsule_cylinder_batch
                .get_pairs()
                .len()
                == 2
        );
        let mut cb = CallbackTest::new(pair_id);
        batches.execute(&container, &mut cb);
        assert!(cb.call_back_count == 1);
        assert!(cb.convex_results.len() == pair_id);

        for i in (1..pair_id).step_by(2) {
            let pair_a = cb.convex_results.get(i).expect("pair should exist");
            let pair_b = cb.convex_results.get(i + 1).expect("pair should exist");
            check_manifold(pair_a, pair_b);
        }
    }

    #[test]
    #[wasm_bindgen_test]
    fn test_batches_push_infinite_plane() {
        use glam_det::f32::Point3;

        let _ = env_logger::builder().is_test(true).try_init();

        let infinite_plane = InfinitePlane::default();
        let capsule = Capsule::new(0.5f32, 0.5f32);
        let cube = Cuboid::new(Vec3::new(0.5f32, 0.5f32, 0.5f32));
        let cylinder = Cylinder::new(0.5, 0.5);
        let sphere = Sphere::new(0.5f32);

        let v0 = Point3::new(0.0, 1.0, 0.0);
        let v1 = Point3::new(1.0, 0.0, 0.0);
        let v2 = Point3::new(-1.0, 0.0, 0.0);
        let v3 = Point3::new(0.0, 0.0, 1.0);
        let points = vec![v0, v1, v2, v3];
        let convex_hull = ConvexHull::new_unchecked(&points);
        let mut container = ShapeContainer::default();
        let convex_hull_id = container.add(convex_hull);

        let offset_b = Vec3::new(0.0f32, -1.0f32, 0.0f32);
        let orientation_a = UnitQuat::default();
        let orientation_b = UnitQuat::default();

        let mut batches = Batches::default();
        let mut pair_id = 0_usize;

        batches.push_test(
            pair_id,
            &Shape::Capsule(capsule),
            &Shape::InfinitePlane(infinite_plane),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::InfinitePlane(infinite_plane),
            &Shape::Capsule(capsule),
            -offset_b,
            orientation_b,
            orientation_a,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Sphere(sphere),
            &Shape::InfinitePlane(infinite_plane),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::InfinitePlane(infinite_plane),
            &Shape::Sphere(sphere),
            -offset_b,
            orientation_b,
            orientation_a,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Cuboid(cube),
            &Shape::InfinitePlane(infinite_plane),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::InfinitePlane(infinite_plane),
            &Shape::Cuboid(cube),
            -offset_b,
            orientation_b,
            orientation_a,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::Cylinder(cylinder),
            &Shape::InfinitePlane(infinite_plane),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::InfinitePlane(infinite_plane),
            &Shape::Cylinder(cylinder),
            -offset_b,
            orientation_b,
            orientation_a,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::ConvexHull(convex_hull_id),
            &Shape::InfinitePlane(infinite_plane),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0f32,
        );

        pair_id += 1;
        batches.push_test(
            pair_id,
            &Shape::InfinitePlane(infinite_plane),
            &Shape::ConvexHull(convex_hull_id),
            -offset_b,
            orientation_b,
            orientation_a,
            &container,
            0.0f32,
        );

        pair_id += 1;
        assert!(
            batches
                .shape_batches
                .capsule_infinite_plane_batch
                .get_pairs()
                .len()
                == 2
        );
        assert!(
            batches
                .shape_batches
                .sphere_infinite_plane_batch
                .get_pairs()
                .len()
                == 2
        );
        assert!(
            batches
                .shape_batches
                .cuboid_infinite_plane_batch
                .get_pairs()
                .len()
                == 2
        );
        assert!(
            batches
                .shape_batches
                .cylinder_infinite_plane_batch
                .get_pairs()
                .len()
                == 2
        );
        assert!(
            batches
                .shape_batches
                .convex_hull_infinite_plane_batch
                .get_pairs()
                .len()
                == 2
        );
        let mut cb = CallbackTest::new(pair_id);

        batches.execute(&container, &mut cb);
        assert!(cb.call_back_count == 0);
        assert!(cb.convex_results.len() == pair_id);
        for i in (0..pair_id).step_by(2) {
            let pair_a = cb.convex_results.get(i).expect("pair should exist");
            let pair_b = cb.convex_results.get(i + 1).expect("pair should exist");
            check_manifold(pair_a, pair_b);
        }
    }

    fn cuboid_to_convex_hull(cuboid: Cuboid) -> ConvexHull {
        let v0 = cuboid.get_vertex(0);
        let v1 = cuboid.get_vertex(1);
        let v2 = cuboid.get_vertex(2);
        let v3 = cuboid.get_vertex(3);
        let v4 = cuboid.get_vertex(4);
        let v5 = cuboid.get_vertex(5);
        let v6 = cuboid.get_vertex(6);
        let v7 = cuboid.get_vertex(7);

        let points = vec![v0, v1, v2, v3, v4, v5, v6, v7];
        ConvexHull::new_unchecked(&points)
    }
    // here the new tootbird rule will let that deep intesect case faild,need some time to resarch
    // this,so ignore this test case here
    // issue #1883
    #[ignore]
    #[test]
    //#[wasm_bindgen_test]
    fn push_convex_hull_pair() {
        let _ = env_logger::builder().is_test(true).try_init();

        let mut batches = Batches::default();
        let mut container = ShapeContainer::default();
        let cuboid = Cuboid::new(Vec3::new(1.0, 1.0, 1.0));
        let convex_hull_0 = cuboid_to_convex_hull(cuboid);
        let support_point_convex = convex_hull_0.support_point(
            Vec3::Z,
            &Isometry3::from_translation(Vec3::new(-0.0, 0.0, 0.0)),
        );
        let support_point_cuboid = cuboid.support_point(
            Vec3::Z,
            &Isometry3::from_translation(Vec3::new(-0.0, 0.0, 0.0)),
        );
        assert_relative_eq!(
            support_point_convex.point.as_vec3().length(),
            support_point_cuboid.point.as_vec3().length()
        );

        let convex_hull_1 = cuboid_to_convex_hull(Cuboid::new(Vec3::new(1.0, 1.0, 1.0)));
        let shape_a_id = container.add(convex_hull_0);
        let shape_b_id = container.add(convex_hull_1);
        let pair_id = 0_usize;
        let shape_a = Shape::ConvexHull(shape_a_id);
        let shape_b = Shape::ConvexHull(shape_b_id);
        let transform_a = Isometry3::from_rotation_translation(
            UnitQuat::from_euler_default(
                45f32.to_radians(),
                45f32.to_radians(),
                45f32.to_radians(),
            ),
            Vec3::new(1.2, 0.0, 0.0),
        );
        let transform_b = Isometry3::from_rotation_translation(
            UnitQuat::from_euler_default(0f32.to_radians(), 0f32.to_radians(), 0.0),
            Vec3::new(0.0, 0.0, 0.0),
        );

        let mut call_back = CallbackTest::new(pair_id + 1);

        batches.push_test(
            pair_id,
            &shape_a,
            &shape_b,
            transform_b.translation - transform_a.translation,
            transform_a.rotation,
            transform_b.rotation,
            &container,
            0.0001f32,
        );
        call_back.pairs.push((transform_a, transform_b));

        batches.execute(&container, &mut call_back);
        let manifold = &call_back.convex_results[0];
        assert_eq!(manifold.count, 1);

        {
            let transform_a = Isometry3::from_rotation_translation(
                UnitQuat::from_euler_default(
                    0f32.to_radians(),
                    0f32.to_radians(),
                    0f32.to_radians(),
                ),
                Vec3::new(2.0, 0.0, 0.0),
            );
            batches.clear();
            batches.push_test(
                pair_id,
                &shape_a,
                &shape_b,
                transform_b.translation - transform_a.translation,
                transform_a.rotation,
                transform_b.rotation,
                &container,
                0.0001f32,
            );
            call_back.pairs.clear();
            call_back.pairs.push((transform_a, transform_b));
            call_back.convex_results.clear();
            batches.execute(&container, &mut call_back);
            let manifold = &call_back.convex_results[0];
            assert_eq!(manifold.count, 0);
        }

        {
            let transform_a = Isometry3::from_rotation_translation(
                UnitQuat::from_euler_default(
                    0f32.to_radians(),
                    0f32.to_radians(),
                    0f32.to_radians(),
                ),
                Vec3::new(0.8, 0.1, 0.0),
            );
            batches.clear();
            batches.push_test(
                pair_id,
                &shape_a,
                &shape_b,
                transform_b.translation - transform_a.translation,
                transform_a.rotation,
                transform_b.rotation,
                &container,
                0.0001f32,
            );
            call_back.pairs.clear();
            call_back.pairs.push((transform_a, transform_b));
            call_back.convex_results.clear();
            batches.execute(&container, &mut call_back);
            let manifold = &call_back.convex_results[0];
            assert_eq!(manifold.count, 4);
        }
    }

    #[test]
    #[wasm_bindgen_test]
    fn push_sphere_convex_hull_pair() {
        let _ = env_logger::builder().is_test(true).try_init();

        let cube = Cuboid::new_xyz(2.0, 2.0, 2.0);
        let convex_hull = cuboid_to_convex_hull(cube);
        let sphere = Sphere::new(1.0f32);
        let mut batches = Batches::default();
        let mut container = ShapeContainer::default();
        let convex_id = container.add(convex_hull);
        let shape_a = Shape::Sphere(sphere);
        let shape_b = Shape::ConvexHull(convex_id);
        let pair_id = 0;
        let mut call_back = CallbackTest::new(pair_id + 1);

        let transform_a = Isometry3::from_rotation_translation(
            UnitQuat::from_xyzw_unchecked(0.50218f32, 0.50218f32, 0.49781f32, 0.49781f32),
            Vec3::new(0.0, 0.0, 0.0),
        );
        let transform_b = Isometry3::from_rotation_translation(
            UnitQuat::from_xyzw_unchecked(0.382_683_46_f32, 0.0, 0.0, 0.923_879_5f32),
            Vec3::new(0.0, 2.0, 0.0),
        );

        batches.push_test(
            pair_id,
            &shape_a,
            &shape_b,
            transform_b.translation - transform_a.translation,
            transform_a.rotation,
            transform_b.rotation,
            &container,
            0.1f32,
        );
        call_back.pairs.push((transform_a, transform_b));
        call_back.resize(call_back.pairs.len());
        batches.execute(&container, &mut call_back);
        assert_eq!(call_back.convex_results.len(), 1);
        assert_eq!(call_back.convex_results[0].count, 1);
        assert_relative_eq!(
            call_back.convex_results[0].normal,
            UnitVec3::new_normalized(0.0, -1.0, 0.0),
            epsilon = 0.0001
        );
        assert_relative_eq!(
            call_back.convex_results[0].contacts[0].offset_a,
            Vec3::new(0.00046813488, 1.0, -1.0686476e-7),
            epsilon = 1e-3_f32,
        );
        assert_relative_eq!(
            call_back.convex_results[0].contacts[0].depth,
            0.414_213_6_f32,
            epsilon = 1e-3_f32,
        );
        assert_eq!(call_back.convex_results[0].contacts[0].feature_id, 0);
        batches.clear();
        batches.push_test(
            pair_id,
            &shape_b,
            &shape_a,
            transform_a.translation - transform_b.translation,
            transform_b.rotation,
            transform_a.rotation,
            &container,
            0.1f32,
        );
        call_back.pairs.clear();
        call_back.pairs.push((transform_b, transform_a));
        call_back.convex_results.clear();
        call_back.resize(call_back.pairs.len());
        batches.execute(&container, &mut call_back);
        assert_eq!(call_back.convex_results.len(), 1);
        assert_eq!(call_back.convex_results[0].count, 1);
        assert_relative_eq!(
            call_back.convex_results[0].normal,
            -UnitVec3::new_normalized(0.0, -1.0, 0.0),
            epsilon = 1e-3_f32,
        );
        assert_eq!(call_back.convex_results[0].contacts[0].feature_id, 0);
    }

    #[test]
    #[wasm_bindgen_test]
    fn push_capsule_convex_hull_pair() {
        let _ = env_logger::builder().is_test(true).try_init();

        let mut batches = Batches::default();
        let mut container = ShapeContainer::default();
        let cuboid = Cuboid::new(Vec3::new(2.0, 2.0, 2.0));
        let convex_hull_0 = cuboid_to_convex_hull(cuboid);
        let shape_b_id = container.add(convex_hull_0);
        let capsule = Capsule::new(1.0, 1.0);
        let shape_a = Shape::Capsule(capsule);
        let shape_b = Shape::ConvexHull(shape_b_id);
        let pair_id = 0;
        let mut call_back = CallbackTest::new(pair_id + 1);

        let transform_a = Isometry3::from_rotation_translation(
            UnitQuat::from_xyzw_unchecked(0.50218f32, 0.50218f32, 0.49781f32, 0.49781f32),
            Vec3::new(0.0, 0.0, 0.0),
        );
        let transform_b = Isometry3::from_rotation_translation(
            UnitQuat::from_euler_default(0f32.to_radians(), 0f32.to_radians(), 0f32.to_radians()),
            Vec3::new(0.0, 0.0, 2.0),
        );
        batches.push_test(
            pair_id,
            &shape_a,
            &shape_b,
            transform_b.translation - transform_a.translation,
            transform_a.rotation,
            transform_b.rotation,
            &container,
            0.0001f32,
        );
        call_back.pairs.push((transform_a, transform_b));
        call_back.resize(call_back.pairs.len());
        batches.execute(&container, &mut call_back);
        let result = &call_back.convex_results[0];
        assert_relative_eq!(result.normal, UnitVec3::new_normalized(0.0, 0.0, -1.0));
        assert_eq!(result.count, 1);
        assert_relative_eq!(
            result.contacts[0].offset_a,
            Vec3::new(0.008739829_f32, 9.536_743E-7_f32, 1.999_960_9_f32)
        );
        assert_relative_eq!(result.contacts[0].depth, 0.9999609_f32);
        assert_eq!(result.contacts[0].feature_id, 1);
        assert_relative_eq!(
            result.offset_b,
            transform_b.translation - transform_a.translation
        );
        batches.clear();
        batches.push_test(
            pair_id,
            &shape_b,
            &shape_a,
            transform_a.translation - transform_b.translation,
            transform_b.rotation,
            transform_a.rotation,
            &container,
            0.0001f32,
        );
        call_back.pairs.clear();
        call_back.pairs.push((transform_b, transform_a));
        call_back.convex_results.clear();
        call_back.resize(call_back.pairs.len());
        batches.execute(&container, &mut call_back);
        let result = &call_back.convex_results[0];
        assert_relative_eq!(result.normal, -UnitVec3::new_normalized(0.0, 0.0, -1.0));
        assert_eq!(result.count, 1);
    }

    #[test]
    #[wasm_bindgen_test]
    fn push_cylinder_convex_hull_pair() {
        let _ = env_logger::builder().is_test(true).try_init();

        let mut batches = Batches::default();
        let mut container = ShapeContainer::default();
        let cuboid = Cuboid::new(Vec3::new(1.0, 1.0, 1.0));
        let convex_hull_0 = cuboid_to_convex_hull(cuboid);
        let shape_b_id = container.add(convex_hull_0);
        let cylinder = Cylinder::new(0.5, 1.0);
        let shape_a = Shape::Cylinder(cylinder);
        let shape_b = Shape::ConvexHull(shape_b_id);
        let pair_id = 0;
        let mut call_back = CallbackTest::new(pair_id + 1);

        let transform_a =
            Isometry3::from_rotation_translation(UnitQuat::default(), Vec3::new(0.0, 0.0, 0.0));
        let transform_b = Isometry3::from_rotation_translation(
            UnitQuat::default(),
            Vec3::new(0.0, 1.0 + 0.001, 0.0),
        );
        batches.push_test(
            pair_id,
            &shape_a,
            &shape_b,
            transform_b.translation - transform_a.translation,
            transform_a.rotation,
            transform_b.rotation,
            &container,
            0.1,
        );
        call_back.pairs.push((transform_a, transform_b));
        batches.execute(&container, &mut call_back);
        let result = &call_back.convex_results[0];
        assert_eq!(result.count, 4);

        batches.clear();
        batches.push_test(
            pair_id,
            &shape_b,
            &shape_a,
            transform_a.translation - transform_b.translation,
            transform_b.rotation,
            transform_a.rotation,
            &container,
            0.1,
        );
        call_back.pairs.push((transform_b, transform_a));
        batches.execute(&container, &mut call_back);
        let result = &call_back.convex_results[0];
        assert_eq!(result.count, 4);
    }

    #[test]
    #[wasm_bindgen_test]
    fn push_cuboid_convex_hull_pair() {
        let _ = env_logger::builder().is_test(true).try_init();

        let mut batches = Batches::default();
        let mut container = ShapeContainer::default();
        let cuboid = Cuboid::UNIT;
        let convex_hull_0 = cuboid_to_convex_hull(Cuboid::new(Vec3::new(2.0, 2.0, 2.0)));
        let shape_b_id = container.add(convex_hull_0);
        let shape_a = Shape::Cuboid(cuboid);
        let shape_b = Shape::ConvexHull(shape_b_id);
        let pair_id = 0;
        let mut call_back = CallbackTest::new(pair_id + 1);

        let transform_a = Isometry3::from_rotation_translation(
            UnitQuat::from_euler_default(0f32.to_radians(), 0f32.to_radians(), 0f32.to_radians()),
            Vec3::new(0.0, 0.0, 0.0),
        );
        let transform_b = Isometry3::from_rotation_translation(
            UnitQuat::from_euler_default(0f32.to_radians(), 0f32.to_radians(), 0f32.to_radians()),
            Vec3::new(0.0, 0.76f32, -0.8f32),
        );
        batches.push_test(
            pair_id,
            &shape_a,
            &shape_b,
            transform_b.translation - transform_a.translation,
            transform_a.rotation,
            transform_b.rotation,
            &container,
            0.0001f32,
        );
        call_back.pairs.push((transform_a, transform_b));
        call_back.resize(call_back.pairs.len());
        batches.execute(&container, &mut call_back);
        let result = &call_back.convex_results[0];
        assert_relative_eq!(result.normal, UnitVec3::new_normalized(0.0, 0.0, 1.0));
        assert_eq!(result.count, 4);
        assert_relative_eq!(
            result.contacts[0].offset_a,
            Vec3::new(0.5, -0.24000001, -0.5)
        );
        assert_relative_eq!(result.contacts[0].depth, 0.7_f32);
        assert_relative_eq!(
            result.offset_b,
            transform_b.translation - transform_a.translation
        );
        batches.clear();
        batches.push_test(
            pair_id,
            &shape_b,
            &shape_a,
            transform_a.translation - transform_b.translation,
            transform_b.rotation,
            transform_a.rotation,
            &container,
            0.0001f32,
        );
        call_back.pairs.clear();
        call_back.pairs.push((transform_b, transform_a));
        call_back.convex_results.clear();
        call_back.resize(call_back.pairs.len());
        batches.execute(&container, &mut call_back);
        let result = &call_back.convex_results[0];
        assert_relative_eq!(result.normal, -UnitVec3::new_normalized(0.0, 0.0, 1.0));
        assert_eq!(result.count, 4);
    }

    #[test]
    #[wasm_bindgen_test]
    #[allow(clippy::float_cmp)]
    fn normal_score_test() {
        let normal0 = Vec3::new(0.0, 0.0, 1.0);
        let normal1 = Vec3::new(0.0, 0.0, -1.0);
        let normal2 = Vec3::new(0.0, 1.0, 0.0);
        let normal3 = Vec3::new(0.0, 0.0, 0.99999);
        let normal4 = Vec3::new(0.0, 0.0, 0.99998);
        let tolerance = 0.00001_f32;
        let score = normal_score(normal0, normal0, tolerance);
        assert_relative_eq!(score, -1e-5_f32);
        let score = normal_score(normal0, normal1, tolerance);
        assert_eq!(score, 2.00001_f32);
        let score = normal_score(normal0, normal2, tolerance);
        assert_eq!(score, 1.0_f32);
        let score = normal_score(normal0, normal3, tolerance);
        assert_eq!(score, 0.0_f32);
        let score = normal_score(normal0, normal4, tolerance);
        assert_relative_eq!(score, 1e-5_f32);
    }

    #[test]
    #[wasm_bindgen_test]
    #[allow(clippy::float_cmp)]
    fn offset_score_test() {
        let offset0 = Vec3::new(0.0, 0.0, 0.0);
        let offset1 = Vec3::new(0.0, 0.0, 1.0);
        let offset2 = Vec3::new(0.0, 0.0, 0.5);
        let max_len_sqr = 1.0;
        let score = offset_score(offset0, offset0, max_len_sqr);
        assert_eq!(score, -1e-6);
        let score = offset_score(offset1, offset1, max_len_sqr);
        assert_eq!(score, -1e-6);
        let score = offset_score(offset1, offset2, max_len_sqr);
        assert_relative_eq!(score, 0.249, epsilon = 1e-3);
    }

    #[test]
    #[wasm_bindgen_test]
    fn reduce() {
        let mut context = ConcaveReductionContext::default();
        let concave_manifold = context.reduce();
        assert_eq!(concave_manifold.count, 0);
        context.count = 1;
        let one_contact = OneContact {
            offset_b: Vec3::new(-22.51, -0.18914255, -16.51),
            normal: UnitVec3::from_array_unchecked([-0.48282743, 0.7305857, 0.48282743]),
            offset_a: Vec3::new(0.120706856, -0.18264642, -0.120706856),
            depth: f32::NAN,
            feature_id: 5,
        };
        context.manifolds.push(one_contact);
        let concave_manifold = context.reduce();
        assert_eq!(concave_manifold.count, 0);
        context.manifolds.clear();
        context.count = 3;

        let one_contact = OneContact {
            offset_b: Vec3::new(-22.51, -0.18914255, -16.51),
            normal: UnitVec3::from_array_unchecked([-0.0, 0.99999994, -0.0]),
            offset_a: Vec3::new(0.0, -0.24999999, 0.0),
            depth: 0.060857445,
            feature_id: 0,
        };
        context.manifolds.push(one_contact);
        let one_contact = OneContact {
            offset_b: Vec3::new(-21.51, -1.189_142_6, -16.51),
            normal: UnitVec3::from_array_unchecked([-0.0, 0.99999994, -0.0]),
            offset_a: Vec3::new(0.0, -0.24999999, 0.0),
            depth: 0.0001,
            feature_id: 0,
        };
        context.manifolds.push(one_contact);
        let one_contact = OneContact {
            offset_b: Vec3::new(-22.51, -0.18914255, -16.51),
            normal: UnitVec3::from_array_unchecked([-0.48282743, 0.7305857, 0.48282743]),
            offset_a: Vec3::new(0.120706856, -0.18264642, -0.120706856),
            depth: 0.0001,
            feature_id: 5,
        };
        context.manifolds.push(one_contact);
        let concave_manifold = context.reduce();
        assert!(concave_manifold.count > 0);
        for i in 0..concave_manifold.count {
            let one_contact = &concave_manifold.manifolds[i];
            assert!(one_contact.depth > 0.0);
        }
    }

    #[test]
    #[wasm_bindgen_test]
    #[allow(clippy::float_cmp)]
    fn concave_manifold_get() {
        let manifold = ConcaveManifold::default();
        let feature_id = manifold.get_contact_feature_id(0);
        assert_eq!(feature_id, 0);
        let offset_a = manifold.get_contact_offset_a(0);
        assert_eq!(offset_a, Vec3::ZERO);
        let depth = manifold.get_contact_depth(0);
        assert_eq!(depth, 0.0f32);
        let normal = manifold.get_contact_normal(0);
        assert_eq!(normal, UnitVec3::X);
    }

    #[derive(Debug, Clone, Copy)]
    struct TestShape;

    #[derive(Clone)]
    struct TestShapePlugin {
        pub sphere_test_shape_batch: SphereIncludingPairBatch<ComplexShapeId>,
        pub capsule_test_shape_batch: StandardPairBatch<Capsule, ComplexShapeId>,
        pub cuboid_test_shape_batch: StandardPairBatch<Cuboid, ComplexShapeId>,
    }
    impl TestShapePlugin {
        fn new() -> Self {
            TestShapePlugin {
                sphere_test_shape_batch: SphereIncludingPairBatch::new(4),
                capsule_test_shape_batch: StandardPairBatch::new(4),
                cuboid_test_shape_batch: StandardPairBatch::new(4),
            }
        }
    }
    impl collision_plugin::CollisionShapePlugin for TestShapePlugin {
        fn clear_pairs(&mut self) {}

        fn on_pair_generate(
            &mut self,
            ctx: &mut PairGenerateContext,
            contact_context: &ContactContext,
            _shapes: &ShapeContainer,
        ) {
            let offset_b = *contact_context.offset_b;
            let orientation_a = *contact_context.orientation_a;
            let orientation_b = *contact_context.orientation_b;
            let id = *ctx.complex_shape_id;
            let speculative_margin = ctx.speculative_margin;
            let flip = ctx.flip_mask;
            let reduction_id = ctx.create_reduction_pair();
            match ctx.shape {
                Shape::Sphere(sphere_a) => {
                    self.sphere_test_shape_batch.push(SphereIncludingPair {
                        a: *sphere_a,
                        b: id,
                        flip_mask: flip,
                        offset_b,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
                }
                Shape::Capsule(capsule_a) => {
                    self.capsule_test_shape_batch.push(StandardPair {
                        a: *capsule_a,
                        b: id,
                        flip_mask: flip,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
                }
                Shape::Cuboid(cuboid_a) => {
                    self.cuboid_test_shape_batch.push(StandardPair {
                        a: *cuboid_a,
                        b: id,
                        flip_mask: flip,
                        offset_b,
                        orientation_a,
                        orientation_b,
                        speculative_margin,
                        reduction_id,
                    });
                }
                _ => {
                    todo!()
                }
            }
        }

        fn plugin_id(&self) -> u64 {
            TestShape::PLUGIN_ID
        }

        fn solve_collision(
            &self,
            _container: Option<&ShapeContainer>,
            _reduction_contex: &mut BatchesReductionContext,
            callback: &mut dyn CollisionCallBack,
        ) {
            let fake_contact = ConvexContact {
                offset_a: vec3(1.0, 0.0, 0.0),
                depth: 1.0,
                feature_id: 0,
            };
            let contacts = [
                fake_contact,
                ConvexContact::default(),
                ConvexContact::default(),
                ConvexContact::default(),
            ];

            let fake_manifold = ConvexContactManifold {
                offset_b: vec3(1.0, 2.0, 3.0),
                normal: UnitVec3 {
                    x: 0.0,
                    y: 1.0,
                    z: 0.0,
                },
                count: 1,
                contacts,
            };

            callback.on_pair_complete(0, ManifoldRef::Convex(&fake_manifold));
        }
    }
    impl ShapePlugin for TestShape {
        const PLUGIN_ID: u64 = 0xc54a471a;
    }
    impl ComputeAabb3 for TestShape {
        fn compute_aabb(&self) -> phys_geom::Aabb3 {
            phys_geom::Aabb3::default()
        }
    }
    impl Expansion for TestShape {
        fn max_radius_and_max_angular_expansion(&self) -> (f32, f32) {
            (0.0, 0.0)
        }
    }
    impl Raycast for TestShape {
        fn raycast(
            &self,
            _local_ray: crate::Ray,
            _max_distance: f32,
            _discard_inside_hit: bool,
        ) -> std::option::Option<RaycastHitResult> {
            None
        }
    }
    impl ContainsPoint for TestShape {
        fn contains_point_with_threshold(
            &self,
            _local_point: Point3,
            _threshold: f32,
        ) -> crate::ContainsResult {
            crate::ContainsResult::Outside
        }
    }
    impl SignedDistanceToPoint for TestShape {
        fn signed_distance_to_point(&self, _local_point: Point3) -> f32 {
            0.0
        }
    }
    impl ComputeVolume for TestShape {
        fn compute_volume(&self) -> phys_geom::math::Real {
            0.0
        }
    }
    impl ComplexShapeTrait for TestShape {}

    #[test]
    #[wasm_bindgen_test]
    fn test_push_custom_shape() {
        let _ = env_logger::builder().is_test(true).try_init();

        let cuboid = Cuboid::new(Vec3::new(1.0, 2.0, 3.0));
        let sphere = Sphere::new(1.0);
        let capsule = Capsule::new(1.0, 0.5);
        let offset_b = Vec3::new(1.0, 2.0, 3.0);
        let orientation_a = UnitQuat::from_axis_angle(UnitVec3::X, std::f32::consts::PI);
        let orientation_b = UnitQuat::from_axis_angle(UnitVec3::Y, std::f32::consts::PI);

        let mut batches = Batches::default();
        batches.register_plugin(TestShapePlugin::new());

        let mut container = ShapeContainer::default();
        let shape_id = container.add(TestShape);

        let mut pair_id = 0_usize;

        batches.push_test(
            pair_id,
            &Shape::Custom {
                plugin_id: TestShape::PLUGIN_ID,
                complex_shape_id: shape_id,
            },
            &Shape::Cuboid(cuboid),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0,
        );

        pair_id += 1;

        batches.push_test(
            pair_id,
            &Shape::Custom {
                plugin_id: TestShape::PLUGIN_ID,
                complex_shape_id: shape_id,
            },
            &Shape::Capsule(capsule),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0,
        );

        pair_id += 1;

        batches.push_test(
            pair_id,
            &Shape::Custom {
                plugin_id: TestShape::PLUGIN_ID,
                complex_shape_id: shape_id,
            },
            &Shape::Sphere(sphere),
            offset_b,
            orientation_a,
            orientation_b,
            &container,
            0.0,
        );

        pair_id += 1;

        let mut cb = CallbackTest::new(pair_id);
        batches.execute(&container, &mut cb);

        assert_eq!(cb.convex_results.len(), 3);
        assert_eq!(cb.convex_results[0].count, 1);
        assert_relative_eq!(
            cb.convex_results[0].normal,
            UnitVec3::new_normalized(0.0, 1.0, 0.0),
            epsilon = 1e-3_f32,
        );
    }
}

#[cfg(all(feature = "unstable", test))]
mod benches {
    use std::hint::black_box;

    use glam_det::{UnitQuat, Vec3};
    use test::Bencher;

    use crate::convex_contact_manifold::ManifoldRef;
    use crate::{
        Batches, Capsule, CollisionCallBack, ContactManifold, Shape, ShapeContainer, Sphere,
    };

    #[derive(Default)]
    struct BenchCallback {
        result_sum: Vec3,
    }

    impl BenchCallback {
        fn new() -> Self {
            Self {
                result_sum: Vec3::default(),
            }
        }
    }

    impl CollisionCallBack for BenchCallback {
        fn on_pair_complete(&mut self, _pair_id: usize, manifold: ManifoldRef) {
            match manifold {
                ManifoldRef::Convex(manifold) => {
                    for index in 0..manifold.contact_count() {
                        self.result_sum += manifold.get_contact_normal(index)
                            + manifold.get_contact_offset_a(index)
                            + manifold.get_contact_depth(index);
                    }
                }
                ManifoldRef::Concave(_) => {}
            }
        }
    }

    fn bench_narrow_phase(a: &Shape, b: &Shape, bencher: &mut Bencher) {
        let mut batches = Batches::default();
        let container = ShapeContainer::default();

        bencher.iter(|| {
            let pair_count = black_box(1000);
            batches.clear();
            for (pair_id, _) in (0..pair_count).enumerate() {
                batches.push_test(
                    pair_id,
                    a,
                    b,
                    black_box(Vec3::new(0.5, 0.5, 0.5)),
                    black_box(UnitQuat::default()),
                    black_box(UnitQuat::default()),
                    &container,
                    black_box(0.0),
                );
            }

            let mut cb = BenchCallback::new();
            batches.execute(&container, &mut cb);
            black_box(cb.result_sum);
        });
    }

    #[bench]
    fn bench_narrow_phase_sphere(bencher: &mut Bencher) {
        let sphere = &Shape::Sphere(black_box(Sphere::new(1.0)));
        bench_narrow_phase(sphere, sphere, bencher);
    }

    #[bench]
    fn bench_narrow_phase_sphere_capsule(bencher: &mut Bencher) {
        let sphere = &Shape::Sphere(black_box(Sphere::new(1.0)));
        let capsule = &Shape::Capsule(black_box(Capsule::new(0.5, 0.5)));
        bench_narrow_phase(sphere, capsule, bencher);
    }
}