phys-collision 2.0.1-beta.0

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

use glam_det::nums::*;
use glam_det::{Dot, Mat3x4, UnitQuat, UnitQuatx4, UnitVec3x4, Vec3, Vec3x4};

use crate::collision_tasks::cuboid_cuboid_tester_helper::{
    calculate_separating_axis, edges_of_face_b_clip_face_a, transform_candidate_to_manifold,
    vertices_of_face_a_clip_face_b, Candidates, CuboidFace, CuboidFaceInitConfig, EdgeClipContext,
    FacePairInfo, VertexClipContext, LOCAL_X_ID, LOCAL_Y_ID, LOCAL_Z_ID, MAX_U32X4, THREE_U32X4,
};
use crate::collision_tasks::traits::TransformWide;
use crate::collision_tasks::{ShapeTester, ShapeWideTester};
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::CuboidWide;
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairTest, PairWideTest};
use crate::{ConvexContactManifold, Cuboid, ShapeContainer};

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

    // 4 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &CuboidWide,
        b: &CuboidWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        //firstly, we need choose the minimum separating axis (contact normal) from the 15
        // potential separating axes of the two cuboids
        //secondly, we will choose two face from the cuboid pair depending on the angle from
        // contact normal to the face normal
        //thirdly, we will use the edge of face b to clip the face a,that will get 8 contact
        // candidate at most
        //fourthly, we will use the vertexs of face a to clip the face b,that will get 4 contact
        // candidate at most,but if edge contact has 8 candidate already,we will not need point
        // clip.
        // finally,we will reduce the candidate count from 8 to 4 depends on the
        // depth,and the area that construct by the candidate points
        let matrix_a = Mat3x4::from_quat(*contact_context.orientation_a);
        let matrix_b = Mat3x4::from_quat(*contact_context.orientation_b);
        let matrix_a_transpose = matrix_a.transpose();
        let rotation_from_b_2_a_localspace = matrix_a_transpose * matrix_b;
        let offset_b_in_a_localspace = matrix_a_transpose * (contact_context.offset_b);
        //we will calculate in the local space of shape a
        let b_2_a_transform =
            TransformWide::new(offset_b_in_a_localspace, &rotation_from_b_2_a_localspace);
        let mut separating_axis_info = calculate_separating_axis(a, b, &b_2_a_transform);

        let active_lanes =
            u32x4::splat(contact_context.pair_count as u32).gt(u32x4::from([0, 1, 2, 3]));
        let minimum_depth = -contact_context.speculative_margin;
        let allow_contacts = active_lanes & (f32x4::ge(separating_axis_info.depth, minimum_depth));
        if allow_contacts == bool32x4::FALSE {
            manifold.reset(4);
        } else {
            //back to the world space
            let normal_dot_offset_b = separating_axis_info.normal.dot(offset_b_in_a_localspace);
            let should_negate_normal = normal_dot_offset_b.gt(f32x4::ZERO);
            separating_axis_info.normal = UnitVec3x4::lane_select(
                should_negate_normal,
                -separating_axis_info.normal,
                separating_axis_info.normal,
            );
            manifold.normal =
                (matrix_a * separating_axis_info.normal.as_vec3x4()).as_unit_vec3x4_unchecked();
            let face_a = CuboidFace::new(
                manifold.normal,
                &matrix_a,
                &a.half_length,
                false,
                &CuboidFaceInitConfig::default(),
            );

            let mut face_b = CuboidFace::new(
                manifold.normal,
                &matrix_b,
                &b.half_length,
                true,
                &CuboidFaceInitConfig::default(),
            );
            face_b.center += contact_context.offset_b;
            let mut candidates = Candidates::default();

            let axis_id_ax = LOCAL_Z_ID.select(
                face_a.use_x_as_face_normal,
                LOCAL_X_ID.select(face_a.use_y_as_face_normal, LOCAL_Y_ID),
            );
            let axis_id_ay = LOCAL_Y_ID.select(
                face_a.use_x_as_face_normal,
                LOCAL_Z_ID.select(face_a.use_y_as_face_normal, LOCAL_X_ID),
            );
            let axis_id_az = LOCAL_X_ID.select(
                face_a.use_x_as_face_normal,
                LOCAL_Y_ID.select(face_a.use_y_as_face_normal, LOCAL_Z_ID),
            );
            let axis_id_bx = LOCAL_Z_ID.select(
                face_b.use_x_as_face_normal,
                LOCAL_X_ID.select(face_b.use_y_as_face_normal, LOCAL_Y_ID),
            );
            let axis_id_by = LOCAL_Y_ID.select(
                face_b.use_x_as_face_normal,
                LOCAL_Z_ID.select(face_b.use_y_as_face_normal, LOCAL_X_ID),
            );
            let axis_id_bz = LOCAL_X_ID.select(
                face_b.use_x_as_face_normal,
                LOCAL_Y_ID.select(face_b.use_y_as_face_normal, LOCAL_Z_ID),
            );

            let epsilon_scale = f32x4::min(face_a.max_half_span(), face_b.max_half_span());
            let twice_axis_id_bx = axis_id_bx * u32x4::TWO;
            let axis_z_edge_id_contribution = axis_id_bz * THREE_U32X4;
            let edge_id_bx0 = twice_axis_id_bx + axis_id_by + axis_z_edge_id_contribution;
            let edge_id_bx1 =
                twice_axis_id_bx + axis_id_by * THREE_U32X4 + axis_z_edge_id_contribution;
            let twice_axis_id_by = axis_id_by * u32x4::TWO;
            let edge_id_by0 = axis_id_bx + twice_axis_id_by + axis_z_edge_id_contribution;
            let edge_id_by1 =
                axis_id_bx * THREE_U32X4 + twice_axis_id_by + axis_z_edge_id_contribution;
            let vertices_of_a = face_a.vertices();
            let face_pair_info = FacePairInfo {
                a: &face_a,
                b: &face_b,
                contact_normal: &manifold.normal,
            };
            edges_of_face_b_clip_face_a(
                &face_pair_info,
                &vertices_of_a,
                &EdgeClipContext::new(
                    contact_context.pair_count,
                    allow_contacts,
                    epsilon_scale,
                    edge_id_bx0,
                    edge_id_bx1,
                    edge_id_by0,
                    edge_id_by1,
                ),
                &mut candidates,
            );

            let vertex_id00 = MAX_U32X4 - axis_id_az;
            let vertex_id01 = MAX_U32X4 - (axis_id_az + axis_id_ay);
            let vertex_id10 = MAX_U32X4 - (axis_id_az + axis_id_ax);
            let vertex_id11 = MAX_U32X4 - (axis_id_az + axis_id_ax + axis_id_ay);
            vertices_of_face_a_clip_face_b(
                &vertices_of_a,
                &face_pair_info,
                &VertexClipContext::new(
                    contact_context.pair_count,
                    allow_contacts,
                    vertex_id00,
                    vertex_id01,
                    vertex_id10,
                    vertex_id11,
                ),
                &mut candidates,
            );
            candidates.reduce_cuboid_pair(
                &face_pair_info,
                minimum_depth,
                epsilon_scale,
                contact_context.pair_count,
                &mut manifold.contact_exists,
            );
            for (candidate, (offset_a, (depth, feature_id))) in candidates.value.iter().take(4).zip(
                manifold.offset_a.iter_mut().zip(
                    manifold
                        .depth
                        .iter_mut()
                        .zip(manifold.feature_id.iter_mut()),
                ),
            ) {
                transform_candidate_to_manifold(candidate, &face_b, offset_a, depth, feature_id);
            }
            for (offset_a, &depth) in manifold.offset_a.iter_mut().zip(manifold.depth.iter()) {
                *offset_a -= manifold.normal * depth;
            }

            manifold.normal = manifold.normal.as_vec3x4().normalize_to_unit();
        }
    }
}

impl_pair_narrowphase!(Cuboid, Cuboid, CuboidWide, CuboidWide, 4);

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

    use crate::collision_tasks::ShapeWideTester;
    use crate::convex_contact_manifold::Convex4ContactManifoldWide;
    use crate::shapes::CuboidWide;
    use crate::traits::{CreateShapeWide, PairWideTest};
    use crate::{Cuboid, PairTest, ShapeTester};

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

        let cuboid_a = Cuboid::new(Vec3::splat(1.0));
        let cuboid_b = Cuboid::new(Vec3::splat(2.0));
        let transform_a = Isometry3::IDENTITY;
        let transform_b = Isometry3::from_rotation_translation(
            UnitQuat::from_rotation_arc(
                UnitVec3::NEG_Z,
                UnitVec3::new_normalized(-1.0, -1.0, -1.0),
            ),
            Vec3::new(0.0, 0.0, 1.9),
        );
        let a_wide = <CuboidWide as CreateShapeWide<1>>::create([&cuboid_a].into_iter());
        let b_wide = <CuboidWide as CreateShapeWide<1>>::create([&cuboid_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: None,
        };
        ShapeWideTester::test(&a_wide, &b_wide, &contact_context, &mut manifold);
        assert_relative_eq!(manifold.normal.extract_lane(0), UnitVec3::NEG_Z);
        assert_relative_eq!(manifold.offset_a[0].extract_lane(0).z, 0.5f32);
        assert!(!manifold.contact_exists[1].extract(0));
        assert!(!manifold.contact_exists[2].extract(0));
        assert!(!manifold.contact_exists[3].extract(0));
    }

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

        let cuboid_a = Cuboid::new(Vec3::splat(1.0));
        let cuboid_b = Cuboid::new(Vec3::splat(2.0));
        let transform_a = Isometry3::IDENTITY;
        let transform_b = Isometry3::from_translation(Vec3::new(0.0, 0.0, 1.4));
        let a_wide = <CuboidWide as CreateShapeWide<1>>::create([&cuboid_a].into_iter());
        let b_wide = <CuboidWide as CreateShapeWide<1>>::create([&cuboid_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: None,
        };
        ShapeWideTester::test(&a_wide, &b_wide, &contact_context, &mut manifold);
        assert!(manifold.contact_exists[0].extract(0));
        assert!(manifold.contact_exists[1].extract(0));
        assert!(manifold.contact_exists[2].extract(0));
        assert!(manifold.contact_exists[3].extract(0));
        assert_relative_eq!(manifold.normal.extract_lane(0), UnitVec3::NEG_Z);
        assert_relative_eq!(manifold.offset_a[0].extract_lane(0).z, 0.5f32);
        assert_relative_eq!(manifold.offset_a[1].extract_lane(0).z, 0.5f32);
        assert_relative_eq!(manifold.offset_a[2].extract_lane(0).z, 0.5f32);
        assert_relative_eq!(manifold.offset_a[3].extract_lane(0).z, 0.5f32);

        let result = ShapeTester::test(
            cuboid_a,
            cuboid_b,
            speculative_margin.extract(0),
            offset_b.extract_lane(0),
            orientation_a.extract_lane(0),
            orientation_b.extract_lane(0),
            None,
        );
        assert_eq!(result.count, 4);
    }
}