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, u32x4, Bool, Float, Num, NumConstEx, PartialOrdEx};
use glam_det::{
    Cross, Dot, DotClamp, Isometry3, Mat3x4, Point3, UnitQuat, UnitQuatx4, UnitVec3, Vec3, Vec3x4,
};

use super::common::{NormalizeExt, EPS_8};
use super::tootbird;
use crate::collision_tasks::cuboid_cuboid_tester_helper::{CuboidFace, CuboidFaceInitConfig};
use crate::collision_tasks::manifold_candidate_helper::{
    CandidateScalarsReducer, ManifoldCandidateScalarSmallVec,
};
use crate::collision_tasks::traits::Axis::{X, Y, Z};
use crate::collision_tasks::traits::TransformWide;
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::{Convex4ContactManifoldWide, Face, ManifoldCandidate};
use crate::shapes::{ConvexHullWide, CuboidWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairWideTest};
use crate::{
    ConvexContactManifold, ConvexHull, ConvexHullId, Cuboid, PairTest, ShapeContainer, ShapeTester,
};

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

    // 4 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &CuboidWide,
        b: &ConvexHullWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        let cuboid_orientation = Mat3x4::from_quat(*contact_context.orientation_a);
        let hull_orientation = Mat3x4::from_quat(*contact_context.orientation_b);
        let hull_local_cuboid_orientation = hull_orientation.transpose() * cuboid_orientation;
        let container = contact_context
            .complex_shape_container
            .expect("container must be Some");
        let shift_b = b.get_convexhull_shift_wide(container, contact_context.pair_count);
        let shift_b_world_space = hull_orientation.mul_vec3(shift_b);
        let local_offset_b = hull_orientation.transpose() * contact_context.offset_b + shift_b;
        let local_offset_a = -local_offset_b;
        let center_b = b.get_center(container, contact_context.pair_count) - shift_b;
        let init_normal = (local_offset_a - center_b + shift_b).normalize_or(Vec3x4::Y, EPS_8);
        let inactive_lanes =
            u32x4::const_splat(contact_context.pair_count as u32).le(u32x4::from([0, 1, 2, 3]));
        let hull_epsilon_scale = b.estimate_epsilon_scale(inactive_lanes, container);

        let epsilon_scale = a.half_length.max_element().min(hull_epsilon_scale);

        let depth_threshold = -contact_context.speculative_margin;
        let toot_bird_result = tootbird::find_minimum_depth(
            b,
            a,
            &TransformWide::new(local_offset_a, &hull_local_cuboid_orientation),
            init_normal,
            &tootbird::IterContext::new(
                inactive_lanes,
                epsilon_scale * f32x4::splat(1e-8),
                depth_threshold,
                25,
                contact_context.complex_shape_container,
                true,
            ),
        );
        let inactive_lanes = inactive_lanes | toot_bird_result.depth.lt(depth_threshold);
        if inactive_lanes.all() {
            //test coverage in issue #1364
            return;
        }
        let cuboid_face_config = CuboidFaceInitConfig {
            use_x_as_normal_tangent_x_y: [Y, Z],
            use_y_as_normal_tangent_x_y: [Z, X],
            use_z_as_normal_tangent_x_y: [X, Y],
        };
        let mut cuboid_face = CuboidFace::new(
            toot_bird_result.normal,
            &hull_local_cuboid_orientation,
            &a.half_length,
            false,
            &cuboid_face_config,
        );

        cuboid_face.center += local_offset_a;
        let vertices = cuboid_face.vertices();
        let v00 = (vertices.v00).split_soa();
        let v10 = (vertices.v10).split_soa();
        let v01 = (vertices.v01).split_soa();
        let v11 = (vertices.v11).split_soa();
        let edge_dir_00_01 = vertices.edge_dir_00_01.split_soa();
        let edge_dir_01_11 = vertices.edge_dir_01_11.split_soa();
        let edge_dir_11_10 = vertices.edge_dir_11_10.split_soa();
        let edge_dir_10_00 = vertices.edge_dir_10_00.split_soa();

        let bounding_plane_epsilon = epsilon_scale * f32x4::splat(1e-5);
        let mut hull_face_normals = [UnitVec3::default(); 4];
        let contact_normals_in_b_local_space = toot_bird_result.normal.split_soa();
        let mut picked_hull_faces = [usize::MAX; 4];
        let mut max_hull_face_vertex_count = 0_usize;
        for slot_index in 0..contact_context.pair_count {
            if inactive_lanes.extract(slot_index) {
                // TODO: code coverage in issue #1364
                continue;
            }
            let hull_id = b.get_id(slot_index);
            let hull = container
                .get::<ConvexHull>(hull_id)
                .expect("hull must exist");
            let picked_hull_face = hull.pick_representative_face(
                contact_normals_in_b_local_space[slot_index],
                Point3::from_vec3(toot_bird_result.closest_point_on_a.extract_lane(slot_index)),
                bounding_plane_epsilon.extract(slot_index),
            );
            hull_face_normals[slot_index] = picked_hull_face.normal;
            picked_hull_faces[slot_index] = picked_hull_face.face_index;
            let vertices_count = hull.get_vertex_indices(picked_hull_face.face_index).len();
            if vertices_count > max_hull_face_vertex_count {
                max_hull_face_vertex_count = vertices_count;
            }
        }
        let max_contact_count = max_hull_face_vertex_count.max(8);

        let mut candidates = ManifoldCandidateScalarSmallVec::with_capacity(max_contact_count);

        for slot_index in 0..contact_context.pair_count {
            if inactive_lanes.extract(slot_index) {
                // TODO: code coverage in issue #1364
                continue;
            }
            candidates.clear();
            let hull_id = b.get_id(slot_index);
            let hull = container
                .get::<ConvexHull>(hull_id)
                .expect("hull must exist");
            let hull_face_normal_narrow = hull_face_normals[slot_index];
            let contact_normal_narrow = contact_normals_in_b_local_space[slot_index];
            let v00_slot = v00[slot_index];
            let v10_slot = v10[slot_index];
            let v01_slot = v01[slot_index];
            let v11_slot = v11[slot_index];
            let edge_dir_00_01_slot = edge_dir_00_01[slot_index];
            let edge_dir_01_11_slot = edge_dir_01_11[slot_index];
            let edge_dir_11_10_slot = edge_dir_11_10[slot_index];
            let edge_dir_10_00_slot = edge_dir_10_00[slot_index];
            let cuboid_edge_start = Vec3x4::compose_soa(&[v00_slot, v01_slot, v11_slot, v10_slot]);
            let edge_direction = Vec3x4::compose_soa(&[
                edge_dir_00_01_slot,
                edge_dir_01_11_slot,
                edge_dir_11_10_slot,
                edge_dir_10_00_slot,
            ]);
            let contact_normal = Vec3x4::splat_soa(contact_normal_narrow.as_vec3());
            let edge_plane_normal = contact_normal.cross(edge_direction);

            let face_vertex_indices = hull.get_vertex_indices(picked_hull_faces[slot_index]);
            let mut previous_index = face_vertex_indices[face_vertex_indices.len() - 1];
            let hull_face_origin = hull.get_point(previous_index);
            let mut previous_vertex = hull_face_origin;
            let (hull_face_tangent_y, hull_face_tangent_x) =
                hull_face_normal_narrow.any_orthogonal_pair();
            let mut max_vertex_containment_dots = f32x4::ZERO;

            // hull edges clip cuboid_face
            for index in face_vertex_indices {
                let vertex = hull.get_point(*index);
                let hull_edge_offset_narrow = vertex - previous_vertex;
                let hull_edge_start = Vec3x4::splat_soa(previous_vertex.as_vec3());
                let hull_edge_offset = Vec3x4::splat_soa(hull_edge_offset_narrow);
                let hull_edge_plane_normal = hull_edge_offset_narrow.cross(contact_normal_narrow);
                let hull_edge_plane_normal = Vec3x4::splat_soa(hull_edge_plane_normal);
                let hull_edge_start_to_cuboid_edge = cuboid_edge_start - hull_edge_start;
                let cuboid_vertex_containment_dots =
                    hull_edge_start_to_cuboid_edge.dot(hull_edge_plane_normal);

                max_vertex_containment_dots =
                    max_vertex_containment_dots.max(cuboid_vertex_containment_dots);

                let numerator = hull_edge_start_to_cuboid_edge.dot(edge_plane_normal);
                let denominator = edge_plane_normal.dot(hull_edge_offset);
                let edge_intersections = numerator / denominator;

                let (mut earliest_exit, mut latest_entry) =
                    get_earliest_exit_and_latest_entry(numerator, denominator, edge_intersections);

                latest_entry = latest_entry.max(0.0);
                earliest_exit = earliest_exit.min(1.0);
                let start_id = previous_index.index();
                let end_id = index.index();
                let base_featured_id = (start_id ^ end_id) << 8;
                let has_empty_slot = candidates.len() < max_contact_count;
                if earliest_exit >= latest_entry && has_empty_slot {
                    let point = hull_edge_offset_narrow * earliest_exit + previous_vertex
                        - hull_face_origin;

                    let candidate = ManifoldCandidate::new(
                        point.dot(hull_face_tangent_x),
                        point.dot(hull_face_tangent_y),
                        base_featured_id + end_id,
                    );
                    candidates.push(candidate);
                }
                if latest_entry < earliest_exit && latest_entry > 0.0 && has_empty_slot {
                    //test coverage in issue #1364
                    let vertex =
                        hull_edge_offset_narrow * latest_entry + previous_vertex - hull_face_origin;

                    let candidate = ManifoldCandidate::new(
                        vertex.dot(hull_face_tangent_x),
                        vertex.dot(hull_face_tangent_y),
                        base_featured_id + start_id,
                    );
                    candidates.push(candidate);
                }
                previous_index = *index;
                previous_vertex = vertex;
            }

            // cuboid vertices clip hull face
            if candidates.len() < max_contact_count {
                let hull_face_origin = Vec3x4::splat_soa(hull_face_origin.as_vec3());
                let hull_face_normal = Vec3x4::splat_soa(hull_face_normal_narrow.as_vec3());
                let closest_on_hull_to_cuboid_edge_start = cuboid_edge_start - hull_face_origin;

                let vertex_projection_numerator =
                    closest_on_hull_to_cuboid_edge_start.dot(hull_face_normal);
                let vertex_projection_denominator =
                    f32x4::splat(hull_face_normal_narrow.dot_clamp(contact_normal_narrow));
                let vertex_projection_t =
                    vertex_projection_numerator * vertex_projection_denominator.recip();
                // normal points from b to a
                let projected_vertex =
                    closest_on_hull_to_cuboid_edge_start - vertex_projection_t * contact_normal;

                let hull_face_tangent_x = Vec3x4::splat_soa(hull_face_tangent_x.as_vec3());
                let hull_face_tangent_y = Vec3x4::splat_soa(hull_face_tangent_y.as_vec3());
                let projected_tangent_x = projected_vertex.dot(hull_face_tangent_x);
                let projected_tangent_y = projected_vertex.dot(hull_face_tangent_y);

                let max_vertex_containment_dots: [f32; 4] = max_vertex_containment_dots.into();
                let projected_tangent_x: [f32; 4] = projected_tangent_x.into();
                let projected_tangent_y: [f32; 4] = projected_tangent_y.into();
                if max_vertex_containment_dots[0] <= 0.0 {
                    let candidate =
                        ManifoldCandidate::new(projected_tangent_x[0], projected_tangent_y[0], 0);
                    candidates.push(candidate);
                }
                for (i, &max_vertex_containment_dot) in
                    max_vertex_containment_dots.iter().enumerate().skip(1)
                {
                    if candidates.len() < max_contact_count && max_vertex_containment_dot <= 0.0f32
                    {
                        let candidate = ManifoldCandidate::new(
                            projected_tangent_x[i],
                            projected_tangent_y[i],
                            i as u32,
                        );
                        candidates.push(candidate);
                    }
                }
            }
            if candidates.is_empty() {
                //just push the tootbird point as a candidate contact
                //TODO:code coverage in Issue #1904
                let closest_a_in_b = toot_bird_result.closest_point_on_a.extract_lane(slot_index);
                let closest_a_in_b = closest_a_in_b - hull_face_origin.as_vec3();
                candidates.push(ManifoldCandidate::new(
                    closest_a_in_b.dot(hull_face_tangent_x.as_vec3()),
                    closest_a_in_b.dot(hull_face_tangent_y.as_vec3()),
                    0,
                ));
            }
            // TODO: code coverage in issue #1364
            // test coverage red in this line ,so add todo here
            let cuboid_face_center = cuboid_face.center.extract_lane(slot_index);
            let cuboid_face_normal = cuboid_face.normal.extract_lane(slot_index);
            let offset_b = contact_context.offset_b.extract_lane(slot_index);
            let slot_hull_orientation = contact_context.orientation_b.extract_lane(slot_index);
            let rotation_to_world =
                Isometry3::from_rotation_translation(slot_hull_orientation, offset_b);
            if !candidates.is_empty() {
                let mut reducer = CandidateScalarsReducer::new(
                    &mut candidates,
                    manifold,
                    slot_index,
                    rotation_to_world,
                );
                let inverse_face_normal_a_dot_local_normal =
                    cuboid_face_normal.dot(contact_normal_narrow).recip();
                let dot_axis = cuboid_face_normal * inverse_face_normal_a_dot_local_normal;
                let face_b = Face {
                    origin: hull_face_origin,
                    tangent_x: hull_face_tangent_x,
                    tangent_y: hull_face_tangent_y,
                };
                reducer.reduce(
                    dot_axis,
                    Point3::from_vec3(cuboid_face_center),
                    &face_b,
                    epsilon_scale.extract(slot_index),
                    depth_threshold.extract(slot_index),
                );
            }
        }
        manifold.normal = (hull_orientation * toot_bird_result.normal).as_unit_vec3x4_unchecked();
        for (offset, depth) in manifold.offset_a.iter_mut().zip(manifold.depth.iter()) {
            *offset -= manifold.normal * depth - shift_b_world_space;
        }
    }
}

fn get_earliest_exit_and_latest_entry(
    numerator: f32x4,
    denominator: f32x4,
    edge_intersections: f32x4,
) -> (f32, f32) {
    let numerator: [f32; 4] = numerator.into();
    let denominator: [f32; 4] = denominator.into();
    let edge_intersections: [f32; 4] = edge_intersections.into();
    let mut latest_entry;
    let mut earliest_exit;
    if denominator[0] < 0.0 {
        latest_entry = edge_intersections[0];
        earliest_exit = f32::MAX;
    } else if denominator[0] > 0.0 {
        latest_entry = f32::MIN;
        earliest_exit = edge_intersections[0];
    } else if numerator[0] < 0.0 {
        earliest_exit = f32::MIN;
        latest_entry = f32::MAX;
    } else {
        latest_entry = f32::MIN;
        earliest_exit = f32::MAX;
    }

    for i in 1..4 {
        if denominator[i] < 0.0 {
            if edge_intersections[i] > latest_entry {
                latest_entry = edge_intersections[i];
            }
        } else if denominator[i] > 0.0 {
            if edge_intersections[i] < earliest_exit {
                // TODO: code coverage in issue #1364
                earliest_exit = edge_intersections[i];
            }
        } else if numerator[i] < 0.0 {
            earliest_exit = f32::MIN;
            latest_entry = f32::MAX;
        }
    }

    (earliest_exit, latest_entry)
}
impl_pair_narrowphase!(Cuboid, ConvexHullId, CuboidWide, ConvexHullWide, 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, CuboidWide};
    use crate::traits::{CreateShapeWide, PairWideTest};
    use crate::{ConvexHull, Cuboid, 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(1.0));
        let b = Cuboid::new(Vec3::splat(2.0));
        let points_b = (0..8).map(|i| b.get_vertex(i)).collect::<Vec<_>>();
        let b = ConvexHull::new_unchecked(&points_b);
        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.4));
        let a_wide = <CuboidWide 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 {
            offset_b: &offset_b,
            orientation_a: &orientation_a,
            orientation_b: &orientation_b,
            speculative_margin,
            complex_shape_container: Some(&container),
            pair_count: 1,
        };

        ShapeWideTester::test(&a_wide, &b_wide, &contact_context, &mut manifold);
        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);
    }
}