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

use super::common::EPS_5;
use super::tootbird::find_minimum_depth;
use crate::{
    CandidateScalarsReducer, ComplexShapeId, ContactContextTester as ContactContext,
    ContactManifoldWide, Convex4ContactManifoldWide, ConvexContactManifold, ConvexHull,
    ConvexHullWide, CreateShapeWide, Face, IterContext, ManifoldCandidate,
    ManifoldCandidateScalarSmallVec, MinkowskiSupportWide, PairTest, PairWideTest, ShapeContainer,
    ShapeTester, ShapeWideTester, TransformWide, Triangle, TriangleWide,
};

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

    // 4 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &TriangleWide,
        b: &ConvexHullWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        let container = contact_context
            .complex_shape_container
            .expect("ShapeContainer is required for ConvexHullWide");

        let triangle_orientation = Mat3x4::from_quat(*contact_context.orientation_a);
        let hull_orientation = Mat3x4::from_quat(*contact_context.orientation_b);

        // Work in b's local space.
        // The transpose of a rotation matrix is its inverse matrix
        let rotation_a = hull_orientation.transpose() * triangle_orientation;
        let shift_b = b.get_convexhull_shift_wide(container, contact_context.pair_count);
        let rotated_shift_b = hull_orientation.mul_vec3(shift_b);
        let local_offset_b = hull_orientation.transpose() * contact_context.offset_b + shift_b;

        let local_a = rotation_a * a.a.as_vec3x4();
        let local_b = rotation_a * a.b.as_vec3x4();
        let local_c = rotation_a * a.c.as_vec3x4();
        let centroid = (local_a + local_b + local_c) * f32x4::splat(1.0 / 3.0);

        // The barycentric coordinates in b's local space.
        let centroid_a = local_a - centroid;
        let centroid_b = local_b - centroid;
        let centroid_c = local_c - centroid;

        // The triangle center in b's local space.
        let local_triangle_center = centroid - local_offset_b;

        // Triangle edge in b's local space.
        let triangle_ab = centroid_b - centroid_a;
        let triangle_bc = centroid_c - centroid_b;
        let triangle_ca = centroid_a - centroid_c;

        // Triangle vertex in b's local space.
        let triangle_a = centroid_a + local_triangle_center;
        let triangle_b = centroid_b + local_triangle_center;
        let triangle_c = centroid_c + local_triangle_center;

        let mut triangle_normal = Vec3x4::cross(triangle_ab, triangle_ca);
        let triangle_normal_length = triangle_normal.length();
        triangle_normal *= triangle_normal_length.recip();

        //Check if the hull's position is within the triangle and below the triangle plane. If
        // so, we can ignore it.
        let hull_to_triangle_dot = Vec3x4::dot(triangle_normal, local_triangle_center);
        let hull_below_plane = hull_to_triangle_dot.gt(f32x4::ZERO);

        let egde_plane_ab = Vec3x4::cross(triangle_ab, triangle_normal);
        let egde_plane_bc = Vec3x4::cross(triangle_bc, triangle_normal);
        let egde_plane_ca = Vec3x4::cross(triangle_ca, triangle_normal);

        let ab_plane_test = Vec3x4::dot(egde_plane_ab, triangle_a);
        let bc_plane_test = Vec3x4::dot(egde_plane_bc, triangle_b);
        let ca_plane_test = Vec3x4::dot(egde_plane_ca, triangle_c);

        let hull_inside_triangle_edge_planes = ab_plane_test.le(f32x4::ZERO)
            & bc_plane_test.le(f32x4::ZERO)
            & ca_plane_test.le(f32x4::ZERO);
        let convexhull_inside_and_below_triangle =
            hull_inside_triangle_edge_planes & hull_below_plane;
        let mut inactive_lanes =
            u32x4::splat(contact_context.pair_count as u32).le(u32x4::from([0, 1, 2, 3]));
        inactive_lanes = inactive_lanes | convexhull_inside_and_below_triangle;

        if inactive_lanes.all() {
            manifold.reset(4);
            return;
        }

        // Set up initial normal.
        let length = local_triangle_center.length();
        let mut initial_normal = local_triangle_center * length.recip();
        let use_initial_sample_fallback = length.lt(f32x4::splat(1e-10f32));
        initial_normal =
            Vec3x4::lane_select(use_initial_sample_fallback, Vec3x4::Y, initial_normal);

        //First attempt to locate a support point.
        let negated_triangle_normal = -triangle_normal;
        let hull_support_along_negated_triangle_normal = b
            .support_point_local(
                negated_triangle_normal,
                contact_context.complex_shape_container,
            )
            .point
            .as_vec3x4();
        let negated_triangle_normal_support =
            hull_support_along_negated_triangle_normal - local_triangle_center;
        let triangle_face_depth =
            Vec3x4::dot(negated_triangle_normal_support, negated_triangle_normal);

        //Check if the extreme point on the hull is contained within the bounds of the triangle
        // face. If it is, there is no need for a full depth refinement.
        let closest_to_a = triangle_a - hull_support_along_negated_triangle_normal;
        let closest_to_b = triangle_b - hull_support_along_negated_triangle_normal;
        let closest_to_c = triangle_c - hull_support_along_negated_triangle_normal;
        let extreme_ab_plane_test = Vec3x4::dot(egde_plane_ab, closest_to_a);
        let extreme_bc_plane_test = Vec3x4::dot(egde_plane_bc, closest_to_b);
        let extreme_ca_plane_test = Vec3x4::dot(egde_plane_ca, closest_to_c);
        let triangle_normal_is_minimal = !convexhull_inside_and_below_triangle
            & extreme_ab_plane_test.le(f32x4::ZERO)
            & extreme_bc_plane_test.le(f32x4::ZERO)
            & extreme_ca_plane_test.le(f32x4::ZERO);
        let depth_threshold = -contact_context.speculative_margin;
        let skip_depth_refine = triangle_normal_is_minimal | inactive_lanes;
        let hull_epsilon_scale = b.estimate_epsilon_scale(inactive_lanes, container);

        //Get proper local normal and depth.
        let unit_negated_triangle_normal = negated_triangle_normal.as_unit_vec3x4_unchecked();
        let (local_normal, closest_on_hull, depth) = {
            if skip_depth_refine.all() {
                (
                    unit_negated_triangle_normal,
                    hull_support_along_negated_triangle_normal,
                    triangle_face_depth,
                )
            } else {
                let local_offset_a = -local_offset_b;
                let a_2_b_transform =
                    contact_context.orientation_b.inverse() * contact_context.orientation_a; // triangle rotation in cylinder space
                let toot_bird_result = find_minimum_depth(
                    b,
                    a,
                    &TransformWide::new(local_offset_a, &a_2_b_transform),
                    initial_normal,
                    &IterContext::new(
                        inactive_lanes,
                        hull_epsilon_scale * EPS_5,
                        depth_threshold,
                        25,
                        contact_context.complex_shape_container,
                        false,
                    ),
                );

                (
                    UnitVec3x4::lane_select(
                        skip_depth_refine,
                        unit_negated_triangle_normal,
                        toot_bird_result.normal,
                    ),
                    Vec3x4::lane_select(
                        skip_depth_refine,
                        hull_support_along_negated_triangle_normal,
                        toot_bird_result.closest_point_on_a,
                    ),
                    f32x4::select(
                        triangle_face_depth,
                        skip_depth_refine,
                        toot_bird_result.depth,
                    ),
                )
            }
        };

        let triangle_normal_dot_local_normal = Vec3x4::dot(triangle_normal, local_normal);

        //If the convex hull is too far away or if it's on the backside of the triangle, don't
        // generate any contacts.
        inactive_lanes = inactive_lanes
            | depth.lt(depth_threshold)
            | triangle_normal_dot_local_normal.gt(-TriangleWide::BACKFACE_THRESHOLD);

        if inactive_lanes.all() {
            manifold.reset(4);
            return;
        }

        let inactive: [bool; 4] = inactive_lanes.into();
        let slots_closest_on_hull = Point3x4::from_vec3x4(closest_on_hull).split_soa();
        let slots_local_normal = local_normal.split_soa();
        let mut maximum_face_vertex_count = 0_usize;

        let mut slots_hull_face_normal = [UnitVec3::default(); 4];
        let mut slots_hull_face_index = [0_usize; 4];

        //Now, We have a local_normal and a closest_on_b, try to find 'contact point'
        //Iterate through each lane and select the most appropriate face based on the direction and
        // the closest point.
        //Cache the results for further process.
        for (i, &hull_b_id) in b.iter_take(contact_context.pair_count).enumerate() {
            if inactive[i] {
                continue;
            }

            let hull_b = container
                .get::<ConvexHull>(hull_b_id)
                .expect("invalid shape id");
            let epsilon_scale = hull_b.cal_estimate_epsilon_scale();
            let bounding_plane_eps = epsilon_scale * 1e-5;
            let slot_closest_on_hull = slots_closest_on_hull[i];
            let slot_local_normal = slots_local_normal[i];

            let face_b = hull_b.pick_representative_face(
                slot_local_normal,
                slot_closest_on_hull,
                bounding_plane_eps,
            );

            let face_index = face_b.face_index;

            //Cache the normal.
            slots_hull_face_normal[i] = face_b.normal;
            //Cache the face index.
            slots_hull_face_index[i] = face_b.face_index;

            //Find the max vertices count.
            maximum_face_vertex_count =
                maximum_face_vertex_count.max(hull_b.get_vertex_indices(face_index).len());
        }

        //Project the triangle onto the convex hull face
        let hull_face_normal = UnitVec3x4::compose_soa(&slots_hull_face_normal);

        let hull_to_a = triangle_a - closest_on_hull;
        let hull_to_b = triangle_b - closest_on_hull;
        let hull_to_c = triangle_c - closest_on_hull;
        let numerator_a_to_hull_face = Vec3x4::dot(hull_to_a, hull_face_normal);
        let numerator_b_to_hull_face = Vec3x4::dot(hull_to_b, hull_face_normal);
        let numerator_c_to_hull_face = Vec3x4::dot(hull_to_c, hull_face_normal);
        let denominator_to_hull_face = Vec3x4::dot(local_normal.as_vec3x4(), hull_face_normal);

        let inverse_denominator_to_hull_face = denominator_to_hull_face.recip();
        let t_a_to_hull_face = numerator_a_to_hull_face * inverse_denominator_to_hull_face;
        let t_b_to_hull_face = numerator_b_to_hull_face * inverse_denominator_to_hull_face;
        let t_c_to_hull_face = numerator_c_to_hull_face * inverse_denominator_to_hull_face;

        let a_on_hull = triangle_a - local_normal * t_a_to_hull_face;
        let b_on_hull = triangle_b - local_normal * t_b_to_hull_face;
        let c_on_hull = triangle_c - local_normal * t_c_to_hull_face;

        let ab_on_hull = b_on_hull - a_on_hull;
        let bc_on_hull = c_on_hull - b_on_hull;
        let ca_on_hull = a_on_hull - c_on_hull;

        let triangle_tangent_x = triangle_ab.normalize().as_unit_vec3x4_unchecked();
        let triangle_tangent_y =
            UnitVec3x4::cross(triangle_tangent_x, triangle_normal).as_unit_vec3x4_unchecked();

        let ab_edge_plane_on_hull = Vec3x4::cross(ab_on_hull, hull_face_normal);
        let bc_edge_plane_on_hull = Vec3x4::cross(bc_on_hull, hull_face_normal);
        let ca_edge_plane_on_hull = Vec3x4::cross(ca_on_hull, hull_face_normal);

        let inverse_triangle_normal_dot_local_normal = triangle_normal_dot_local_normal.recip();

        let maximum_contact_count = maximum_face_vertex_count.max(6);
        let mut candidates = ManifoldCandidateScalarSmallVec::with_capacity(maximum_contact_count);

        let speculative_margins: [f32; 4] = contact_context.speculative_margin.into();

        //Check convex hull per lane.
        for (slot_index, &hull_b_id) in b.iter_take(contact_context.pair_count).enumerate() {
            if inactive[slot_index] {
                continue;
            }

            candidates.clear();

            let hull_b = container
                .get::<ConvexHull>(hull_b_id)
                .expect("invalid shape id");

            let slot_face_normal = slots_hull_face_normal[slot_index];
            let slot_local_normal = local_normal.extract_lane(slot_index);
            let face_vertex_indices = hull_b.get_vertex_indices(slots_hull_face_index[slot_index]);

            //use extract or split_soa previously?
            let slot_triangle_a = triangle_a.extract_lane(slot_index);
            let slot_triangle_b = triangle_b.extract_lane(slot_index);
            let slot_triangle_c = triangle_c.extract_lane(slot_index);
            let slot_triangle_normal = triangle_normal.extract_lane(slot_index);
            let slot_inverse_triangle_normal_dot_local_normal =
                inverse_triangle_normal_dot_local_normal.extract(slot_index);
            let slot_a_on_hull = a_on_hull.extract_lane(slot_index);
            let slot_b_on_hull = b_on_hull.extract_lane(slot_index);
            let slot_c_on_hull = c_on_hull.extract_lane(slot_index);

            let slot_triangle_tangent_x = triangle_tangent_x.extract_lane(slot_index);
            let slot_triangle_tangent_y = triangle_tangent_y.extract_lane(slot_index);

            let slot_ab_edge_plane_on_hull = ab_edge_plane_on_hull.extract_lane(slot_index);
            let slot_bc_edge_plane_on_hull = bc_edge_plane_on_hull.extract_lane(slot_index);
            let slot_ca_edge_plane_on_hull = ca_edge_plane_on_hull.extract_lane(slot_index);

            let previous_index = face_vertex_indices[face_vertex_indices.len() - 1];
            let mut previous_vertex = hull_b.get_point(previous_index).as_vec3();
            //let mut candidate_count = 0;

            let mut latest_entry_ab = f32::MIN;
            let mut latest_entry_bc = f32::MIN;
            let mut latest_entry_ca = f32::MIN;

            let mut earliest_exit_ab = f32::MAX;
            let mut earliest_exit_bc = f32::MAX;
            let mut earliest_exit_ca = f32::MAX;

            let slot_ab_on_hull = slot_b_on_hull - slot_a_on_hull;
            let slot_bc_on_hull = slot_c_on_hull - slot_b_on_hull;
            let slot_ca_on_hull = slot_a_on_hull - slot_c_on_hull;
            let slot_triangle_ab = slot_triangle_b - slot_triangle_a;
            let slot_triangle_bc = slot_triangle_c - slot_triangle_b;
            let slot_triangle_ca = slot_triangle_a - slot_triangle_c;

            //test triangle edges per hull face edges
            for index in face_vertex_indices {
                let vertex = hull_b.get_point(*index).as_vec3();
                let hull_edge_offset = vertex - previous_vertex;

                previous_vertex = vertex;

                let ap = vertex - slot_a_on_hull;
                let bp = vertex - slot_b_on_hull;
                let cp = vertex - slot_c_on_hull;

                //Check if the hull face vertex is inside the triangle. If so, record it.
                let vertex_contained = Vec3::dot(ap, slot_ab_edge_plane_on_hull) < 0.0
                    && Vec3::dot(bp, slot_bc_edge_plane_on_hull) < 0.0
                    && Vec3::dot(ap, slot_ca_edge_plane_on_hull) < 0.0;
                if vertex_contained && candidates.len() < maximum_contact_count {
                    let projection_t = Vec3::dot(vertex - slot_triangle_a, slot_triangle_normal)
                        * slot_inverse_triangle_normal_dot_local_normal;
                    let projected_vertex = vertex - slot_local_normal * projection_t;
                    let to_vertex = projected_vertex - slot_triangle_a;

                    candidates.push(ManifoldCandidate::new(
                        Vec3::dot(to_vertex, slot_triangle_tangent_x),
                        Vec3::dot(to_vertex, slot_triangle_tangent_y),
                        6 + slot_index as u32,
                    ));
                }

                let hull_edge_plane_normal = Vec3::cross(hull_edge_offset, slot_local_normal);

                //3 edges
                (latest_entry_ab, earliest_exit_ab) = two_lines_intersection_test(
                    ap,
                    slot_ab_on_hull,
                    hull_edge_plane_normal,
                    latest_entry_ab,
                    earliest_exit_ab,
                );

                (latest_entry_bc, earliest_exit_bc) = two_lines_intersection_test(
                    bp,
                    slot_bc_on_hull,
                    hull_edge_plane_normal,
                    latest_entry_bc,
                    earliest_exit_bc,
                );

                (latest_entry_ca, earliest_exit_ca) = two_lines_intersection_test(
                    cp,
                    slot_ca_on_hull,
                    hull_edge_plane_normal,
                    latest_entry_ca,
                    earliest_exit_ca,
                );
            }

            latest_entry_ab = latest_entry_ab.max(0.0);
            latest_entry_bc = latest_entry_bc.max(0.0);
            latest_entry_ca = latest_entry_ca.max(0.0);
            earliest_exit_ab = earliest_exit_ab.min(1.0);
            earliest_exit_bc = earliest_exit_bc.min(1.0);
            earliest_exit_ca = earliest_exit_ca.min(1.0);

            //Check for intersections between the three edges of the triangle and the convex hull
            // face
            for (entry, exit, edge, offset, feature_id_offset) in [
                (
                    latest_entry_ab,
                    earliest_exit_ab,
                    slot_triangle_ab,
                    Vec3::ZERO,
                    0,
                ),
                (
                    latest_entry_bc,
                    earliest_exit_bc,
                    slot_triangle_bc,
                    slot_triangle_ab,
                    2,
                ),
                (
                    latest_entry_ca,
                    earliest_exit_ca,
                    slot_triangle_ca,
                    -slot_triangle_ca,
                    4,
                ),
            ] {
                if exit >= entry && candidates.len() < maximum_contact_count {
                    let point = edge * exit + offset;
                    candidates.push(ManifoldCandidate::new(
                        Vec3::dot(point, slot_triangle_tangent_x),
                        Vec3::dot(point, slot_triangle_tangent_y),
                        feature_id_offset,
                    ));
                }
                if entry < exit && entry > 0.0 && candidates.len() < maximum_contact_count {
                    let point = edge * entry + offset;
                    candidates.push(ManifoldCandidate::new(
                        Vec3::dot(point, slot_triangle_tangent_x),
                        Vec3::dot(point, slot_triangle_tangent_y),
                        1 + feature_id_offset,
                    ));
                }
            }

            if candidates.is_empty() {
                continue;
            }

            let transform_b = Isometry3::from_rotation_translation(
                contact_context.orientation_b.extract_lane(slot_index),
                contact_context.offset_b.extract_lane(slot_index)
                    + rotated_shift_b.extract_lane(slot_index),
            );

            let slot_inverse_face_normal_dot_local_normal =
                Vec3::dot(slot_face_normal.as_vec3(), slot_local_normal).recip();

            //Reducer use `face_center_a_to_face_center_b` to calculate depth. Thus we should
            // reverse face normal.
            let dot_axis = -slot_face_normal * slot_inverse_face_normal_dot_local_normal;

            let mut reducer =
                CandidateScalarsReducer::new(&mut candidates, manifold, slot_index, transform_b);

            //face center a should be the convex hull face.
            let face_center_a = Point3::from_vec3(previous_vertex);
            let min_depth = -speculative_margins[slot_index];
            let epsilon_scale = hull_b.cal_estimate_epsilon_scale();

            //The tangent plane(face) where the candidates at.
            let face_b = Face {
                origin: Point3::from_vec3(slot_triangle_a),
                tangent_x: slot_triangle_tangent_x,
                tangent_y: slot_triangle_tangent_y,
            };

            reducer.reduce(dot_axis, face_center_a, &face_b, epsilon_scale, min_depth);
        }
        //Fill normal
        manifold.normal = (hull_orientation * local_normal).as_unit_vec3x4_unchecked();
    }
}

fn two_lines_intersection_test(
    ap: Vec3,
    ab: Vec3,
    normal: Vec3,
    mut entry: f32,
    mut exit: f32,
) -> (f32, f32) {
    let numerator = Vec3::dot(ap, normal);
    let denominator = Vec3::dot(ab, normal);

    if denominator < 0.0 {
        if entry * denominator > numerator {
            entry = numerator / denominator;
        }
    } else if denominator > 0.0 {
        if exit * denominator > numerator {
            exit = numerator / denominator;
        }
    } else if denominator == 0.0 && numerator < 0.0 {
        entry = f32::MAX;
        exit = f32::MIN;
    }

    (entry, exit)
}
impl_pair_narrowphase!(Triangle, ComplexShapeId, TriangleWide, ConvexHullWide, 4);

mod tests {
    #[test]
    fn test_line_entry() {
        use approx_det::assert_relative_eq;
        use glam_det::Vec3;
        let p = Vec3::new(0.0, 1.0, 0.0);
        let a = Vec3::new(-1.0, 0.0, 0.0);
        let b = Vec3::new(1.0, 0.0, 0.0);
        let ab = b - a;
        let ap = p - a;
        let normal = Vec3::new(-1.0, 0.0, 0.0);
        let entry = f32::MIN;
        let exit = f32::MAX;
        let (entry, exit) = super::two_lines_intersection_test(ap, ab, normal, entry, exit);
        assert_relative_eq!(entry, 0.5f32);
        assert_relative_eq!(exit, f32::MAX);
    }

    #[test]
    fn test_line_exit() {
        use approx_det::assert_relative_eq;
        use glam_det::Vec3;
        let p = Vec3::new(0.0, 1.0, 0.0);
        let a = Vec3::new(-1.0, 0.0, 0.0);
        let b = Vec3::new(1.0, 0.0, 0.0);
        let ab = b - a;
        let ap = p - a;
        let normal = Vec3::new(1.0, 0.0, 0.0);
        let entry = f32::MIN;
        let exit = f32::MAX;
        let (entry, exit) = super::two_lines_intersection_test(ap, ab, normal, entry, exit);
        assert_relative_eq!(entry, f32::MIN);
        assert_relative_eq!(exit, 0.5f32);
    }
}