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::num_traits::*;
use glam_det::nums::{f32x4, u32x4};
use glam_det::{Cross, Dot, Mat3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4};

use crate::collision_tasks::{ShapeTester, ShapeWideTester};
use crate::convex_contact_manifold::{Convex4ContactManifoldWide, ConvexContactManifold};
use crate::shapes::{Cuboid, CuboidWide, InfinitePlaneWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairTest, PairWideTest};
use crate::{InfinitePlane, ShapeContainer};

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

    // 4 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &CuboidWide,
        _b: &InfinitePlaneWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        // transform box into plane space, get box center's penetration
        let ra = Mat3x4::from_quat(*contact_context.orientation_a);
        let rb = Mat3x4::from_quat(*contact_context.orientation_b);
        let world_to_plane = rb.transpose();
        let box_to_plane = world_to_plane * ra;
        let box_center_in_plane = world_to_plane * (-*contact_context.offset_b);
        let box_center_depth = box_center_in_plane.y;

        //     4+------+0
        //     /|     /|
        //    / |    / |
        //   / 6+---/--+2
        // 5+------+1 /    y   z
        //  | /    | /     |  /
        //  |/     |/      |/
        // 7+------+3      *---x
        let vertices = [
            Vec3x4::new(a.half_length.x, a.half_length.y, a.half_length.z),
            Vec3x4::new(a.half_length.x, a.half_length.y, -a.half_length.z),
            Vec3x4::new(a.half_length.x, -a.half_length.y, a.half_length.z),
            Vec3x4::new(a.half_length.x, -a.half_length.y, -a.half_length.z),
            Vec3x4::new(-a.half_length.x, a.half_length.y, a.half_length.z),
            Vec3x4::new(-a.half_length.x, a.half_length.y, -a.half_length.z),
            Vec3x4::new(-a.half_length.x, -a.half_length.y, a.half_length.z),
            Vec3x4::new(-a.half_length.x, -a.half_length.y, -a.half_length.z),
        ];
        // caculate 8 vetices projection on plane's normal in cube space,
        // plus the results with box_center_depth to get the vertices penetration
        // let plane_normal_in_cube = box_to_plane.conjugate() * UnitVec3x4::Y;
        let plane_normal_in_cube = box_to_plane.transpose().y_axis;
        let dx = plane_normal_in_cube.x * a.half_length.x;
        let dy = plane_normal_in_cube.y * a.half_length.y;
        let dz = plane_normal_in_cube.z * a.half_length.z;

        let active_lanes =
            u32x4::splat(contact_context.pair_count as u32).gt(u32x4::from([0, 1, 2, 3]));

        let depths = [
            (dx + dy + dz + box_center_depth).select(active_lanes, f32x4::MAX),
            (dx + dy - dz + box_center_depth).select(active_lanes, f32x4::MAX),
            (dx - dy + dz + box_center_depth).select(active_lanes, f32x4::MAX),
            (dx - dy - dz + box_center_depth).select(active_lanes, f32x4::MAX),
            (-dx + dy + dz + box_center_depth).select(active_lanes, f32x4::MAX),
            (-dx + dy - dz + box_center_depth).select(active_lanes, f32x4::MAX),
            (-dx - dy + dz + box_center_depth).select(active_lanes, f32x4::MAX),
            (-dx - dy - dz + box_center_depth).select(active_lanes, f32x4::MAX),
        ];

        // create the return manifold
        manifold.feature_id[0] = u32x4::ZERO;
        manifold.feature_id[1] = u32x4::ONE;
        manifold.feature_id[2] = u32x4::const_splat(2);
        manifold.feature_id[3] = u32x4::const_splat(3);

        // check all vertices depth,
        // if the vertex's depth is less than the margin(penetrate plane),
        // store the negative depth(required by other caculations)
        // and vertex's world position into the candidates array,
        // increment contact counts.
        let mut contact_counts = u32x4::ZERO;
        let mut candidates = [(f32x4::MAX, Vec3x4::default()); 8];
        for i in 0..8 {
            let depth_lt_margin = depths[i].lt(contact_context.speculative_margin);
            candidates[i].0 = -depths[i];
            candidates[i].1 = contact_context.orientation_a * vertices[i];
            // TODO:  test_cuboid_infinite_plane_006_3contacts_rotate_x_45_z_45 failed for ra
            // candidates[i].1 = ra * vertices[i];
            contact_counts = (contact_counts + u32x4::ONE).select(depth_lt_margin, contact_counts);
        }

        // If no contact, early exit
        let no_contact = contact_counts == u32x4::ZERO;
        if no_contact {
            manifold.reset(4);
            return;
        }

        // contact normal
        let normal = rb * InfinitePlaneWide::NORMAL.as_vec3x4();
        manifold.normal = normal.as_unit_vec3x4_unchecked();

        // set how many contact exist, the max is 4,
        // if more than 4 vertices penetrate the plane, reduce and change later
        manifold.contact_exists[0] = contact_counts.ge(u32x4::ONE);
        manifold.contact_exists[1] = contact_counts.ge(u32x4::const_splat(2));
        manifold.contact_exists[2] = contact_counts.ge(u32x4::const_splat(3));
        manifold.contact_exists[3] = contact_counts.ge(u32x4::const_splat(4));

        // the following code find and store 4 candidates:
        // the deepest contact point, manifold.offset_a[0]
        // the furthest contact point from manifold.offset_a[0], contact1
        // and the furthest contact points from manifold.offset_a[0]-contac1 segment, contact2 and
        // contact3 since we do all the calculation in wide operation, we need to loop
        // through all 8 candidates.

        let mut c0_i = u32x4::const_splat(8);
        let mut c1_i = u32x4::const_splat(8);
        let mut c2_i = u32x4::const_splat(8);
        let mut c3_i = u32x4::const_splat(8);

        // contact 0
        // first find the deepest contact vertex,
        // store the position and depth in manifold.offset_a[0] and manifold.depth[0]
        manifold.depth[0] = f32x4::MIN;
        candidates.iter().enumerate().for_each(
            #[inline]
            |(i, candidate)| {
                let dx4 = candidate.0;
                let d_gt_margin = dx4.gt(-contact_context.speculative_margin);
                let d_gt = dx4.gt(manifold.depth[0]);
                let update_c0 = d_gt_margin & d_gt;
                manifold.depth[0] = dx4.select(update_c0, manifold.depth[0]);
                manifold.offset_a[0] =
                    Vec3x4::lane_select(update_c0, candidate.1, manifold.offset_a[0]);
                c0_i = u32x4::const_splat(i as u32).select(update_c0, c0_i);
            },
        );

        // contact 1
        // then find the furthest contact vertex from manifold.offset_a[0]
        // store the position and depth in contact1 and c1_d
        let mut max_c0c1 = f32x4::ZERO;
        let mut c1_d = f32x4::default();
        let mut contact1 = Vec3x4::default();

        candidates.iter().enumerate().for_each(
            #[inline]
            |(i, candidate)| {
                let ix4 = u32x4::const_splat(i as u32);
                let ne_c0 = ix4.ne(c0_i);
                let dx4 = candidate.0;
                let d_gt_margin = dx4.gt(-contact_context.speculative_margin);
                let dist = candidate.1.distance_squared(manifold.offset_a[0]);
                let dist_gt_max_c0c1 = dist.gt(max_c0c1);
                let update_c1 = ne_c0 & d_gt_margin & dist_gt_max_c0c1;
                max_c0c1 = dist.select(update_c1, max_c0c1);
                contact1 = Vec3x4::lane_select(update_c1, candidate.1, contact1);
                c1_d = dx4.select(update_c1, c1_d);
                c1_i = ix4.select(update_c1, c1_i);
            },
        );

        // contact 2 & 3
        // calculate the min and max distance point away the segment (manifold.offset_a[0] and
        // contact1) and store the max distance point to contact2, the min distance point to
        // contact3
        let mut max_c2_seg = f32x4::MIN;
        let mut min_c3_seg = f32x4::MAX;
        let mut contact2 = Vec3x4::default();
        let mut contact3 = Vec3x4::default();
        let mut c2_d = f32x4::default();
        let mut c3_d = f32x4::default();
        let seg = contact1 - manifold.offset_a[0];
        let mut seg_norm = seg.cross(normal).normalize_or_zero();
        let norm_eq_zero = seg_norm.cmpeq(Vec3x4::ZERO);
        seg_norm = Vec3x4::select(norm_eq_zero, normal, seg_norm);

        candidates.iter().enumerate().for_each(
            #[inline]
            |(i, candidate)| {
                let dx4 = candidate.0;
                let ix4 = u32x4::const_splat(i as u32);
                let d_gt_margin = dx4.gt(-contact_context.speculative_margin);
                let p_ne_c0 = ix4.ne(c0_i);
                let p_ne_c1 = ix4.ne(c1_i);
                let c0p = candidate.1 - manifold.offset_a[0];
                let dist = c0p.dot(seg_norm);
                let dist_gt_max_c2 = dist.gt(max_c2_seg);
                let dist_lt_min_c3 = dist.lt(min_c3_seg);
                let update_c2 =
                    p_ne_c0 & p_ne_c1 & d_gt_margin & dist_gt_max_c2 & manifold.contact_exists[2];
                let update_c3 =
                    p_ne_c0 & p_ne_c1 & d_gt_margin & dist_lt_min_c3 & manifold.contact_exists[3];
                max_c2_seg = dist.select(update_c2, max_c2_seg);
                min_c3_seg = dist.select(update_c3, min_c3_seg);
                contact2 = Vec3x4::lane_select(update_c2, candidate.1, contact2);
                contact3 = Vec3x4::lane_select(update_c3, candidate.1, contact3);
                c2_d = dx4.select(update_c2, c2_d);
                c3_d = dx4.select(update_c3, c3_d);
                c2_i = ix4.select(update_c2, c2_i);
                c3_i = ix4.select(update_c3, c3_i);
            },
        );

        // after we get 4 candidate contacts, we need to check whether they are the deepest 4
        // contacts,
        // if other contact vertices are deeper than the current candidate contacts, find the
        // closest one and set it to candidate contacts
        let mut closest_1_d = f32x4::MAX;
        let mut closest_2_d = f32x4::MAX;
        let mut closest_3_d = f32x4::MAX;
        manifold.offset_a[1] = contact1;
        manifold.offset_a[2] = contact2;
        manifold.offset_a[3] = contact3;
        manifold.depth[1] = c1_d;
        manifold.depth[2] = c2_d;
        manifold.depth[3] = c3_d;
        candidates.iter().enumerate().for_each(
            #[inline]
            |(i, candidate)| {
                let dx4 = candidate.0;
                let ix4 = u32x4::const_splat(i as u32);
                let p_ne_c0 = ix4.ne(c0_i);
                let p_ne_c1 = ix4.ne(c1_i);
                let p_ne_c2 = ix4.ne(c2_i);
                let p_ne_c3 = ix4.ne(c3_i);
                // not current contacts flag
                let p_ne = p_ne_c0 & p_ne_c1 & p_ne_c2 & p_ne_c3;
                let d_gt_margin = dx4.gt(-contact_context.speculative_margin);

                let dist_1 = candidate.1.distance_squared(contact1);
                let dist_2 = candidate.1.distance_squared(contact2);
                let dist_3 = candidate.1.distance_squared(contact3);

                let dist1_is_closest = dist_1.le(dist_2) & dist_1.le(dist_3);
                let dist2_is_closest = dist_2.le(dist_1) & dist_2.le(dist_3);
                let dist3_is_closest = dist_3.le(dist_1) & dist_3.le(dist_2);
                let d_gt_cc1d = dx4.gt(manifold.depth[1]);
                let d_gt_cc2d = dx4.gt(manifold.depth[2]);
                let d_gt_cc3d = dx4.gt(manifold.depth[3]);

                // update contact1 closest & deeper flag
                let change_1 = p_ne & d_gt_margin & d_gt_cc1d & dist1_is_closest;
                closest_1_d = dist_1.select(change_1, closest_1_d);
                manifold.offset_a[1] =
                    Vec3x4::lane_select(change_1, candidate.1, manifold.offset_a[1]);
                manifold.depth[1] = dx4.select(change_1, manifold.depth[1]);

                // update contact2 closest & deeper flag
                let change_2 = p_ne & d_gt_margin & d_gt_cc2d & dist2_is_closest;
                closest_2_d = dist_2.select(change_2, closest_2_d);
                manifold.offset_a[2] =
                    Vec3x4::lane_select(change_2, candidate.1, manifold.offset_a[2]);
                manifold.depth[2] = dx4.select(change_2, manifold.depth[2]);

                // update contact3 closest & deeper flag
                let change_3 = p_ne & d_gt_margin & d_gt_cc3d & dist3_is_closest;
                closest_3_d = dist_3.select(change_3, closest_3_d);
                manifold.offset_a[3] =
                    Vec3x4::lane_select(change_3, candidate.1, manifold.offset_a[3]);
                manifold.depth[3] = dx4.select(change_3, manifold.depth[3]);
            },
        );
    }
}

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