phys-collision 2.0.1-beta.0

// Copyright (C) 2020-2025 phys-collision authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use glam_det::nums::{f32x4, i32x4, u32x4, Num, NumConstEx, PartialOrdEx};
use glam_det::{Mat3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4};

use super::common::{EPS_5, EPS_8};
use crate::collision_tasks::common::NormalizeExt;
use crate::collision_tasks::traits::TransformWide;
use crate::collision_tasks::{tootbird, ShapeWideTester};
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::{ConvexHullWide, SphereWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairWideTest};
use crate::{ConvexContactManifold, ConvexHullId, PairTest, ShapeContainer, ShapeTester, Sphere};

impl PairWideTest<SphereWide, ConvexHullWide> for ShapeWideTester {
    #[inline]
    fn should_reset_manifold_before_test() -> bool {
        false
    }

    // 1 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &SphereWide,
        b: &ConvexHullWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        const MAXIMUM_ITERRATIONS: usize = 25;
        // Following compute are all in the b local space

        // Get normal_a for tootbird input
        let container = contact_context
            .complex_shape_container
            .expect("ShapeContainer is required for ConvexHullWide");
        let shift_b = b.get_convexhull_shift_wide(container, contact_context.pair_count);
        let shift_b_world_space = contact_context.orientation_b * shift_b;
        let pair_count_i32 =
            i32::try_from(contact_context.pair_count).expect("pair_count must in range");
        let local_offset_b =
            contact_context.orientation_b.inverse() * contact_context.offset_b + shift_b;
        let local_offset_a = -local_offset_b;
        let center_b = b.get_center(container, contact_context.pair_count);
        let normal_a = (local_offset_a - center_b).normalize_or(Vec3x4::Y, EPS_8);

        let inactive_lanes = i32x4::splat(pair_count_i32).le(i32x4::from([0, 1, 2, 3]));
        let hull_epsilon_scale = b.estimate_epsilon_scale(inactive_lanes, container);
        let epsilon_scale = a.radius.min(hull_epsilon_scale);
        let toot_bird_result = tootbird::find_minimum_depth(
            b,
            a,
            &TransformWide::new(local_offset_a, &Mat3x4::IDENTITY),
            normal_a,
            &tootbird::IterContext::new(
                inactive_lanes,
                epsilon_scale * EPS_5,
                -contact_context.speculative_margin,
                MAXIMUM_ITERRATIONS,
                Some(container),
                true,
            ),
        );
        let hull_to_contact = contact_context.orientation_b * toot_bird_result.closest_point_on_a;
        let offset_b = contact_context.offset_b + shift_b_world_space;
        manifold.normal = contact_context.orientation_b * toot_bird_result.normal;
        manifold.offset_a[0] = hull_to_contact + offset_b;
        manifold.offset_a[0] -= manifold.normal * toot_bird_result.depth;
        manifold.feature_id[0] = u32x4::ZERO;
        manifold.depth[0] = toot_bird_result.depth;

        manifold.contact_exists[0] = toot_bird_result
            .depth
            .ge(-contact_context.speculative_margin);
    }
}
impl_pair_narrowphase!(Sphere, ConvexHullId, SphereWide, ConvexHullWide, 1);

#[cfg(test)]
mod tests {
    use approx_det::assert_relative_eq;
    use glam_det::nums::{f32x4, Num};
    use glam_det::{Isometry3, UnitQuatx4, Vec3, Vec3x4};
    use wasm_bindgen_test::wasm_bindgen_test;

    use crate::collision_tasks::ShapeWideTester;
    use crate::convex_contact_manifold::Convex4ContactManifoldWide;
    use crate::shapes::{ConvexHullWide, CuboidExt, SphereWide};
    use crate::traits::{CreateShapeWide, PairWideTest};
    use crate::{ConvexHull, Cuboid, ShapeContainer, Sphere};

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

        let a = Sphere::new(1.0);
        let b = Cuboid::new(Vec3::splat(2.0));
        let points = (0..8).map(|i| b.get_vertex(i)).collect::<Vec<_>>();
        let b = ConvexHull::new_unchecked(&points);
        let mut container = ShapeContainer::default();
        let b = container.add(b);
        let transform_a = Isometry3::IDENTITY;
        let transform_b = Isometry3::from_translation(Vec3::new(0.0, 0.0, 1.8));
        let a_wide = <SphereWide as CreateShapeWide<1>>::create([&a].into_iter());
        let b_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&b].into_iter());
        let speculative_margin = f32x4::splat(0.01);
        let offset_b = Vec3x4::splat_soa(transform_b.translation - transform_a.translation);
        let orientation_a = UnitQuatx4::splat_soa(transform_a.rotation);
        let orientation_b = UnitQuatx4::splat_soa(transform_b.rotation);
        let mut manifold = Convex4ContactManifoldWide::default();
        let contact_context = crate::traits::ContactContext {
            orientation_a: &orientation_a,
            orientation_b: &orientation_b,
            offset_b: &offset_b,
            speculative_margin,
            pair_count: 1,
            complex_shape_container: Some(&container),
        };
        ShapeWideTester::test(&a_wide, &b_wide, &contact_context, &mut manifold);
        assert_relative_eq!(manifold.offset_a[0].extract_lane(0).z, 1.0f32);
    }
}