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, Num};
#[cfg(all(feature = "contact_debug", feature = "qhull"))]
use glam_det::Isometry3;
use glam_det::{Dot, Isometry3x4, Point3, UnitQuat, UnitQuatx4, UnitVec3, Vec3, Vec3x4};

use crate::collision_tasks::manifold_candidate_helper::{
    CandidateScalarsReducer, ManifoldCandidateScalarSmallVec,
};
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::{Convex4ContactManifoldWide, Face, ManifoldCandidate};
#[cfg(all(feature = "contact_debug", feature = "qhull"))]
use crate::debug::{ContactDebugContext, InsightPair, ManifoldInsight, TriangleListWithHighlight};
#[cfg(all(feature = "contact_debug", feature = "qhull"))]
use crate::shapes::BundleIndex;
use crate::shapes::{ConvexHullWide, InfinitePlaneWide};
use crate::traits::{
    ContactContext, ContactManifoldWide, CreateShapeWide, Orientation, PairWideTest,
};
use crate::{
    ConvexContactManifold, ConvexHull, ConvexHullId, InfinitePlane, PairTest, ShapeContainer,
    ShapeTester,
};

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

    // 4 manifold in Convex4ContactManifoldWide
    fn test(
        a: &ConvexHullWide,
        _b: &InfinitePlaneWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        // the logic of this tester is:
        // 1. transform convex hull into infinite plane space,
        // 2. find the convex hull's support point in negative Y direction(opposite of plane's
        // normal),
        // 3. pick the convex hull's face that contains the support point,
        // 4. check all vertices on the picked face, if it penetrates the plane, put it into
        // candidates list,
        // 5. reduce the candidate list based on penetration depth and distance
        let container = contact_context
            .complex_shape_container
            .expect("ShapeContainer is required for ConvexHullWide");
        let shift_a = a.get_convexhull_shift_wide(container, contact_context.pair_count);
        let shift_a_in_world_space = contact_context.orientation_a * shift_a;
        let transform_a = Isometry3x4::from_rotation_translation(
            *contact_context.orientation_a,
            shift_a_in_world_space,
        );
        let transform_b = Isometry3x4::from_rotation_translation(
            *contact_context.orientation_b,
            *contact_context.offset_b,
        );
        let transform_bs = transform_b.split_soa();
        let transform_as = transform_a.split_soa();
        let a_to_b_transform_x4 = transform_b.inverse() * transform_a;
        let a_to_b_transforms = a_to_b_transform_x4.split_soa();
        let speculative_margins: [f32; 4] = contact_context.speculative_margin.into();
        // this tester assumes that input manifold is default

        a.iter_take(contact_context.pair_count)
            .enumerate()
            .for_each(
                #[inline]
                |(i, &a)| {
                    let transform_a = transform_as[i];
                    let transform_b = transform_bs[i];
                    let transform_a_to_b = a_to_b_transforms[i];
                    let convex_a = container.get::<ConvexHull>(a).expect("invalid shape id");
                    let direction =
                        transform_a.inverse_mul_vec3(transform_b.transform_vector3(Vec3::NEG_Y));
                    let support_a = convex_a.support_point_local_narrow(direction).point;
                    let support_a_in_b_local = transform_a_to_b.transform_point3(support_a);
                    if support_a_in_b_local.y <= speculative_margins[i] {
                        let epsilon_scale = convex_a.cal_estimate_epsilon_scale();

                        // convert support point into convex's space, required by
                        // pick_representative_face
                        let face_a = convex_a.pick_representative_face(
                            direction.as_unit_vec3_unchecked(),
                            support_a,
                            epsilon_scale * 1e-3,
                        );
                        let face_a_vertex_indices = convex_a.get_vertex_indices(face_a.face_index);
                        #[cfg(all(feature = "contact_debug", feature = "qhull"))]
                        record_debug_contact_info(
                            contact_context,
                            i,
                            transform_a,
                            convex_a,
                            face_a.face_index,
                            face_a_vertex_indices,
                        );

                        let max_candidate_counts = face_a_vertex_indices.len();
                        let mut candidates =
                            ManifoldCandidateScalarSmallVec::with_capacity(max_candidate_counts);
                        let (infinite_plane_x, infinite_plane_z) = (UnitVec3::X, UnitVec3::Z);
                        let mut feature_id = 0;

                        // check face vertices penetration
                        for bundle_index in face_a_vertex_indices {
                            let mut vertex = convex_a.get_point(*bundle_index);
                            vertex = a_to_b_transforms[i].transform_point3(vertex);
                            if vertex.y <= speculative_margins[i] {
                                candidates.push(ManifoldCandidate::new(
                                    vertex.as_vec3().dot(infinite_plane_x),
                                    vertex.as_vec3().dot(infinite_plane_z),
                                    feature_id,
                                ));
                                feature_id += 1;
                            }
                        }
                        if !candidates.is_empty() {
                            let mut reducer = CandidateScalarsReducer::new(
                                &mut candidates,
                                manifold,
                                i,
                                transform_b,
                            );
                            // data required by the reducer
                            let face_a_normal_in_b_space = a_to_b_transforms[i] * face_a.normal;
                            let inverse_face_normal_a_dot_local_normal =
                                face_a_normal_in_b_space.dot(Vec3::Y).recip();
                            let dot_axis =
                                face_a_normal_in_b_space * inverse_face_normal_a_dot_local_normal;

                            let face_b = Face {
                                origin: Point3::ZERO,
                                tangent_x: infinite_plane_x,
                                tangent_y: infinite_plane_z,
                            };
                            reducer.reduce(
                                dot_axis,
                                support_a_in_b_local,
                                &face_b,
                                epsilon_scale,
                                -speculative_margins[i],
                            );
                        }
                    }
                },
            );
        manifold.normal = contact_context.orientation_b * InfinitePlaneWide::NORMAL;
        manifold
            .offset_a
            .iter_mut()
            .zip(manifold.depth.iter())
            .for_each(
                #[inline]
                |(offset_a, depth)| {
                    *offset_a -= manifold.normal * depth;
                },
            );
    }
}

#[cfg(all(feature = "contact_debug", feature = "qhull"))]
fn record_debug_contact_info(
    contact_context: &ContactContext,
    i: usize,
    transform_a: Isometry3,
    convex_a: &ConvexHull,
    face_index: usize,
    face_a_vertex_indices: &[BundleIndex],
) {
    let results = crate::debug::get_or_init();
    if results.is_err() {
        log::error!("record_debug_contact_info: {}", results.err().unwrap());
        return;
    }
    let mut results = results.unwrap();
    let mut highlight_triangles = TriangleListWithHighlight {
        transform: transform_a,
        ..Default::default()
    };
    let first_face = face_a_vertex_indices.iter().map(|v| v.index());
    highlight_triangles.face_vertex_indices.extend(first_face);
    highlight_triangles.face_indices_start.push(0);

    let neighbours = convex_a.get_neighbours(face_index);
    for (i, &neighbour) in neighbours.iter().enumerate() {
        let neighbour_vertices = convex_a.get_vertex_indices(neighbour as usize);
        let face = neighbour_vertices.iter().map(|v| v.index());
        highlight_triangles.face_vertex_indices.extend(face);
        highlight_triangles.face_indices_start.push((i + 1) as u32);
    }
    let mut debug_context = ContactDebugContext {
        count: 1,
        ..Default::default()
    };
    let mut pair = InsightPair::default();
    pair.value[0] = ManifoldInsight::MeshInsight {
        highlight_triangles,
    };
    let plane_normal = contact_context.orientation_b.extract_lane(i) * Vec3::Y;
    pair.value[1] = ManifoldInsight::Plane {
        normal: plane_normal,
        distance: plane_normal.dot(contact_context.offset_b.extract_lane(i)),
    };
    debug_context.insight_pairs[0] = pair;
    (*results).push(debug_context);
}

impl_pair_narrowphase!(
    ConvexHullId,
    InfinitePlane,
    ConvexHullWide,
    InfinitePlaneWide,
    4
);
#[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, InfinitePlaneWide};
    use crate::traits::{CreateShapeWide, PairWideTest};
    use crate::{ConvexHull, Cuboid, InfinitePlane, ShapeContainer};

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

        let a = Cuboid::new(Vec3::splat(2.0));
        let b = InfinitePlane::default();
        let points_a = (0..8).map(|i| a.get_vertex(i)).collect::<Vec<_>>();
        let a = ConvexHull::new_unchecked(&points_a);
        let mut container = ShapeContainer::default();
        let a = container.add(a);
        let transform_a = Isometry3::from_translation(Vec3::new(0.0, 0.1, 0.0));
        let transform_b = Isometry3::IDENTITY;
        let a_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&a].into_iter());
        let b_wide = <InfinitePlaneWide 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).y, -1.0f32);
        assert_relative_eq!(manifold.offset_a[1].extract_lane(0).y, -1.0f32);
        assert_relative_eq!(manifold.offset_a[2].extract_lane(0).y, -1.0f32);
        assert_relative_eq!(manifold.offset_a[3].extract_lane(0).y, -1.0f32);
    }

    #[cfg(all(feature = "contact_debug", feature = "qhull"))]
    #[test]
    #[wasm_bindgen_test]
    fn test_record_debug_contact_info() {
        let _ = env_logger::builder().is_test(true).try_init();

        // Create a simple cube as convex hull
        let cube = Cuboid::new(Vec3::splat(2.0));
        let points = (0..8).map(|i| cube.get_vertex(i)).collect::<Vec<_>>();
        let convex_hull = ConvexHull::new_unchecked(&points);

        // Create transforms
        let transform_a = Isometry3::from_translation(Vec3::new(0.0, 0.1, 0.0));
        let orientation_a = UnitQuatx4::splat_soa(transform_a.rotation);
        let orientation_b = UnitQuatx4::splat_soa(Isometry3::IDENTITY.rotation);
        let offset_b = Vec3x4::splat_soa(Vec3::ZERO);

        // Create contact context
        let contact_context = crate::traits::ContactContext {
            orientation_a: &orientation_a,
            orientation_b: &orientation_b,
            offset_b: &offset_b,
            speculative_margin: f32x4::splat(0.01),
            pair_count: 1,
            complex_shape_container: None,
        };

        // Call the function with test data
        super::record_debug_contact_info(
            &contact_context,
            0, // index
            transform_a,
            &convex_hull,
            0, // face_index
            [0, 1, 2, 3].map(crate::shapes::BundleIndex::new).as_ref(),
        );

        // Verify the debug results
        let results = crate::debug::get_or_init().unwrap();
        assert!(!results.is_empty());
        // for the other test effect ,the result that pending the vec may be not in the index 0,so
        // we just get the end element here
        let debug_context = &results[results.len() - 1];
        assert_eq!(debug_context.count, 1);

        let pair = &debug_context.insight_pairs[0];
        match &pair.value[0] {
            crate::debug::ManifoldInsight::MeshInsight {
                highlight_triangles,
            } => {
                assert_eq!(highlight_triangles.face_indices_start.len(), 5);
                assert_eq!(highlight_triangles.face_vertex_indices.len(), 20);
            }
            _ => panic!("Expected MeshInsight"),
        }

        match &pair.value[1] {
            crate::debug::ManifoldInsight::Plane { normal, distance } => {
                assert_relative_eq!(normal.y, 1.0);
                assert_relative_eq!(*distance, 0.0);
            }
            _ => panic!("Expected Plane"),
        }
    }
}