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 ::test::{black_box, Bencher};
use glam_det::nums::{bool32x4, f32x4, u32x4, Bool, Num, NumConstEx, PartialOrdEx, Signed};
use glam_det::{Dot, Mat3, Mat3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4};

use crate::collision_tasks::tests::common::{ConvexHullInput, TestInput};
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::{CapsuleWide, ConvexHullWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairWideTest};
use crate::{Capsule, ConvexContactManifold, ConvexHull, ShapeContainer};

#[bench]
fn test_mat3_transpose_mul_vec3(bencher: &mut Bencher) {
    let left = black_box(Mat3x4::from_rotation_x(f32x4::ONE));

    let count = black_box(100);
    let mut value = Vec3x4::ONE;
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                value = left.transpose().mul_vec3(value);
            }
        },
    );
    black_box(value);
}
#[bench]
fn test_mat3_transpose_mul_vec3_2(bencher: &mut Bencher) {
    let left = black_box(Mat3x4::from_rotation_x(f32x4::ONE));

    let count = black_box(100);
    let mut value = Vec3x4::ONE;
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                value.x = left.x_axis.dot(value);
                value.y = left.y_axis.dot(value);
                value.z = left.z_axis.dot(value);
            }
        },
    );
    black_box(value);
}

#[bench]
fn test_inverse_mul_matrix3(bencher: &mut Bencher) {
    let mut left = black_box(Mat3x4::IDENTITY);
    let right = black_box(Mat3x4::from_quat(UnitQuatx4::from_rotation_x(f32x4::ONE)));
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                left = left.transpose() * right;
            }
        },
    );
    black_box(left);
}
#[bench]
fn test_inverse_mul_unit_quat(bencher: &mut Bencher) {
    let mut left = black_box(UnitQuatx4::IDENTITY);
    let right = black_box(UnitQuatx4::from_rotation_x(f32x4::ONE));
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                left = left.inverse() * right;
            }
        },
    );
    black_box(left);
}

#[bench]
fn test_basic_abs2(bencher: &mut Bencher) {
    let mut normal = Vec3x4::new(f32x4::splat(-1.0), f32x4::splat(2.0), f32x4::splat(2.0));
    let count = black_box(1000);
    normal.z = f32x4::ZERO;
    let mut result = Vec3x4::ZERO;
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                let normal = normal.abs();
                result.x = normal.x;
                result.y = normal.y;
            }
        },
    );
    black_box(result.x);
    black_box(result.y);
}
#[bench]
fn test_basic_abs(bencher: &mut Bencher) {
    let mut normal = Vec3x4::new(f32x4::splat(-1.0), f32x4::splat(2.0), f32x4::splat(2.0));
    let count = black_box(1000);
    normal.z = f32x4::ZERO;
    let mut x = f32x4::ZERO;
    let mut y = f32x4::ZERO;
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                x = normal.x.absf();
                y = normal.y.absf();
            }
        },
    );

    black_box(x);
    black_box(y);
}

fn test_match(a: i32) {
    match a {
        1 => {
            black_box(1);
        }
        2 => {
            black_box(2);
        }
        3 => {
            black_box(3);
        }
        _ => {
            black_box(4);
        }
    }
}

#[allow(clippy::unit_arg)]
#[bench]
fn test_basic_match(bencher: &mut Bencher) {
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                black_box(test_match(1));
            }
        },
    );
}
#[allow(clippy::unit_arg)]
#[bench]
fn test_basic_match2(bencher: &mut Bencher) {
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                black_box(1);
            }
        },
    );
}

#[bench]
fn test_basic_matrix_mul_vec3_normally(bencher: &mut Bencher) {
    let normal = Vec3x4::new(f32x4::ZERO, f32x4::splat(2.0), f32x4::splat(2.0));
    let test_matrix = Mat3x4::from_axis_angle(
        Vec3x4::new(f32x4::splat(1.0), f32x4::splat(1.0), f32x4::splat(1.0)).normalize_to_unit(),
        f32x4::splat(1.0),
    );

    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                black_box(test_matrix * normal);
            }
        },
    );
}
#[bench]
fn test_basic_matrix_mul_vec3_trick(bencher: &mut Bencher) {
    let y = f32x4::splat(2.0);
    let z = f32x4::splat(2.0);
    let test_matrix = Mat3x4::from_axis_angle(
        Vec3x4::new(f32x4::splat(1.0), f32x4::splat(1.0), f32x4::splat(1.0)).normalize_to_unit(),
        f32x4::splat(1.0),
    );
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                black_box(Vec3x4::new(
                    y * test_matrix.x_axis.y + z * test_matrix.x_axis.z,
                    y * test_matrix.y_axis.y + z * test_matrix.y_axis.z,
                    y * test_matrix.z_axis.y + z * test_matrix.z_axis.z,
                ));
            }
        },
    );
}

#[bench]
fn test_basic_matrix_mul_vec3(bencher: &mut Bencher) {
    let mut normal = black_box(Vec3x4::ONE);
    let mat = black_box(Mat3x4::IDENTITY);
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                normal = mat * normal;
            }
            black_box(normal);
        },
    );
}

#[bench]
fn test_basic_scale(bencher: &mut Bencher) {
    let mut normal = black_box(Vec3x4::ONE);
    let mut scale = black_box(f32x4::ONE);
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                normal *= scale;
                scale += f32x4::ONE;
                black_box(normal);
                black_box(scale);
            }
        },
    );
}
#[bench]
fn test_basic_scale2(bencher: &mut Bencher) {
    let mut normal = black_box(Vec3x4::ONE);
    let mut scale = black_box(f32x4::ONE);
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                normal *= scale;
                scale += f32x4::ONE;
                black_box(normal);
                black_box(scale);
            }
        },
    );
}

#[bench]
fn test_basic_rotation_mul_matrix(bencher: &mut Bencher) {
    const LEN: usize = 1000_usize;
    let mut normal = black_box([Vec3::ONE; LEN]);
    let rotation = black_box(vec![Mat3::IDENTITY; LEN].into_boxed_slice());
    let count = black_box(LEN);
    bencher.iter(
        #[inline]
        || {
            for i in 0..count {
                normal[i] = rotation[i] * normal[i];
                black_box(normal[i]);
            }
        },
    );
}

#[bench]
fn test_basic_candidate_handle(bencher: &mut Bencher) {
    let masked_contact_count = black_box(u32x4::splat(2));
    let test = black_box(bool32x4::from([true, false, false, true]));
    let count = black_box(8_usize);
    bencher.iter(
        #[inline]
        || {
            for i in 0..count {
                let candidate_exists_v = u32x4::splat(i as u32).lt(masked_contact_count);
                black_box(candidate_exists_v);

                let t = test & candidate_exists_v;
                black_box(t);
            }
        },
    );
}
#[bench]
fn test_basic_candidate_handle2(bencher: &mut Bencher) {
    let masked_contact_count = black_box(u32x4::splat(2));
    let count = black_box(8_usize);
    bencher.iter(
        #[inline]
        || {
            for i in 0..count {
                let candidate_exists_v = u32x4::splat(i as u32).lt(masked_contact_count);
                black_box(candidate_exists_v);
            }
        },
    );
}
#[bench]
fn test_basic_rotation_mul_quat(bencher: &mut Bencher) {
    const LEN: usize = 1000_usize;
    let mut normal = black_box([Vec3::ONE; LEN]);
    let rotation = black_box([UnitQuat::IDENTITY; LEN]);
    let count = black_box(LEN);
    bencher.iter(
        #[inline]
        || {
            for i in 0..count {
                normal[i] = rotation[i] * normal[i];
                black_box(normal[i]);
            }
        },
    );
}

#[bench]
fn test_basic_rotation_mul_matrix_wide(bencher: &mut Bencher) {
    const LEN: usize = 1000_usize;
    let mut normal = black_box(vec![Vec3x4::ONE; LEN].into_boxed_slice());
    let rotation = black_box(vec![Mat3x4::IDENTITY; LEN].into_boxed_slice());
    let count = black_box(LEN);
    bencher.iter(
        #[inline]
        || {
            for i in 0..count {
                normal[i] = rotation[i] * normal[i];
                black_box(normal[i]);
            }
        },
    );
}

#[bench]
fn test_basic_rotation_mul2_wide_quat_wide(bencher: &mut Bencher) {
    const LEN: usize = 1000_usize;
    let mut normal = black_box(vec![Vec3x4::ONE; LEN].into_boxed_slice());
    let rotation = black_box(vec![UnitQuatx4::IDENTITY; LEN].into_boxed_slice());
    let count = black_box(LEN);
    bencher.iter(
        #[inline]
        || {
            for i in 0..count {
                normal[i] = rotation[i] * normal[i];
                black_box(normal[i]);
            }
        },
    );
}

#[bench]
fn test_quat_convert_mat3_mul_vec3_wide(bencher: &mut Bencher) {
    const LEN: usize = 1000_usize;
    let mut normal = black_box(vec![Vec3x4::ONE; LEN].into_boxed_slice());
    let rotation = black_box(vec![UnitQuatx4::IDENTITY; LEN].into_boxed_slice());
    let count = black_box(LEN);
    bencher.iter(
        #[inline]
        || {
            for i in 0..count {
                normal[i] = Mat3x4::from_quat(rotation[i]) * normal[i];
                black_box(normal[i]);
            }
        },
    );
}

#[bench]
fn test_basic_wide_pack_unpack(bencher: &mut Bencher) {
    let slice = include_bytes!(concat!(
        "../tests/resource/",
        "capsule_convex_hull/contact3.json"
    ));
    let data: TestInput<Capsule, ConvexHullInput> =
        serde_json::from_slice(slice).expect("file should be proper JSON");
    let mut container = ShapeContainer::default();
    let convex_hull = ConvexHull::new_unchecked(&data.b.vertices);
    let b_id = container.add(convex_hull);
    let a = <CapsuleWide as CreateShapeWide<4>>::create([data.a, data.a, data.a, data.a].iter());
    let b = <ConvexHullWide as CreateShapeWide<4>>::create([b_id, b_id, b_id, b_id].iter());
    let speculative_margin = f32x4::splat(data.speculative_margin);
    let offset_b = Vec3x4::splat_soa(data.offset_b);
    let orientation_a = UnitQuatx4::splat_soa(data.orientation_a);
    let orientation_b = UnitQuatx4::splat_soa(data.orientation_b);
    let mut result = Convex4ContactManifoldWide::default();
    let pair_count = black_box(4);
    let contact_context = ContactContext {
        orientation_a: &orientation_a,
        orientation_b: &orientation_b,
        offset_b: &offset_b,
        speculative_margin,
        pair_count,
        complex_shape_container: Some(&container),
    };
    ShapeWideTester::test(&a, &b, &contact_context, &mut result);
    let count = black_box(1);
    bencher.iter(
        #[inline]
        || {
            let mut manifold = ConvexContactManifold::default();
            for _ in 0..count {
                let speculative_margin = f32x4::splat(data.speculative_margin);
                let offset_b = Vec3x4::splat_soa(data.offset_b);
                let orientation_a = UnitQuatx4::splat_soa(data.orientation_a);
                let orientation_b = UnitQuatx4::splat_soa(data.orientation_b);
                black_box(speculative_margin);
                black_box(offset_b);
                black_box(orientation_a);
                black_box(orientation_b);

                for i in 0..pair_count {
                    ContactManifoldWide::<ConvexContactManifold>::get_manifold(
                        &result,
                        i,
                        2,
                        &offset_b,
                        &mut manifold,
                    );
                    black_box(&mut manifold);
                }
            }
        },
    );
}

#[bench]
fn capsule_wide_create(bencher: &mut Bencher) {
    let slice = include_bytes!(concat!(
        "../tests/resource/",
        "capsule_convex_hull/contact3.json"
    ));
    let data: TestInput<Capsule, ConvexHullInput> =
        serde_json::from_slice(slice).expect("file should be proper JSON");
    let count = black_box(1);
    let radius = data.a.radius();
    let half_height = data.a.half_height();

    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                let a = CapsuleWide {
                    radius: f32x4::from([radius; 4]),
                    half_height: f32x4::from([half_height; 4]),
                };
                let b = CapsuleWide {
                    radius: f32x4::from([radius; 4]),
                    half_height: f32x4::from([half_height; 4]),
                };

                let speculative_margin = f32x4::splat(data.speculative_margin);
                let offset_b = Vec3x4::splat_soa(data.offset_b);
                let orientation_a = UnitQuatx4::splat_soa(data.orientation_a);
                let orientation_b = UnitQuatx4::splat_soa(data.orientation_b);
                black_box(speculative_margin);
                black_box(offset_b);
                black_box(orientation_a);
                black_box(orientation_b);
                black_box(b);
                black_box(a);
            }
        },
    );
}

#[bench]
fn convex_hull_wide_create(bencher: &mut Bencher) {
    let slice = include_bytes!(concat!(
        "../tests/resource/",
        "capsule_convex_hull/contact3.json"
    ));
    let data: TestInput<Capsule, ConvexHullInput> =
        serde_json::from_slice(slice).expect("file should be proper JSON");
    let mut container = ShapeContainer::default();
    let convex_hull = ConvexHull::new_unchecked(&data.b.vertices);
    let b_id = container.add(convex_hull);
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                let a =
                    <ConvexHullWide as CreateShapeWide<4>>::create([b_id, b_id, b_id, b_id].iter());
                black_box(a);
            }
        },
    );
}
#[bench]
fn convex_hull_wide_create_from_array(bencher: &mut Bencher) {
    let slice = include_bytes!(concat!(
        "../tests/resource/",
        "capsule_convex_hull/contact3.json"
    ));
    let data: TestInput<Capsule, ConvexHullInput> =
        serde_json::from_slice(slice).expect("file should be proper JSON");
    let mut container = ShapeContainer::default();
    let convex_hull = ConvexHull::new_unchecked(&data.b.vertices);
    let b_id = container.add(convex_hull);
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                let a = <ConvexHullWide as CreateShapeWide<4>>::create(
                    [&b_id, &b_id, &b_id, &b_id].into_iter(),
                );
                black_box(a);
            }
        },
    );
}
#[bench]
fn get_pointer_from_container(bencher: &mut Bencher) {
    let slice = include_bytes!(concat!(
        "../tests/resource/",
        "capsule_convex_hull/contact3.json"
    ));
    let data: TestInput<Capsule, ConvexHullInput> =
        serde_json::from_slice(slice).expect("file should be proper JSON");
    let mut container = ShapeContainer::default();
    let convex_hull = ConvexHull::new_unchecked(&data.b.vertices);
    let b_id = container.add(convex_hull);
    let count = black_box(100);
    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                let b = container.get::<ConvexHull>(b_id);
                black_box(b);
            }
        },
    );
}

#[bench]
fn bench_vec3x4_lane_select(bencher: &mut Bencher) {
    const LEN: usize = 60_usize;
    let count = black_box(LEN);
    let cond = black_box(bool32x4::TRUE);
    let if_true = black_box(Vec3x4::default());
    let if_false = black_box(Vec3x4::default());

    bencher.iter(
        #[inline]
        || {
            for _ in 0..count {
                black_box(Vec3x4::lane_select(cond, if_true, if_false));
            }
        },
    );
}