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 std::ops::{BitAnd, Not};

use glam_det::nums::*;
use glam_det::{Cross, Dot, Mat3x4, Point3x4, Vec3x4};

use crate::{
    CapsuleWide, ContactContextTester as ContactContext, Convex4ContactManifoldWide, PairWideTest,
    ShapeWideTester, TriangleWide,
};
/// Refer from 'Real-Time Collision Detection' and BEPU
///
/// The algorithm is divided into two phases
/// * closest points of two lines
/// * clamp
#[inline]
fn closest_points(
    start0: &Vec3x4, // capsule
    direction0: &Vec3x4,
    length0: &f32x4,
    start1: &Vec3x4, // edge
    direction1: &Vec3x4,
    length1: &f32x4,
) -> (f32x4, f32x4, f32x4, f32x4) {
    // closest points of two lines
    let offset = start1 - start0;
    let dir0_dot_offset = direction0.dot(offset);
    let dir1_dot_offset = direction1.dot(offset);
    let dir0_dot_dir1 = direction0.dot(direction1);
    let mut term0 = (dir0_dot_offset - dir1_dot_offset * dir0_dot_dir1)
        / (f32x4::max(f32x4::EPSILON, f32x4::ONE - dir0_dot_dir1 * dir0_dot_dir1));
    let mut term1 = term0 * dir0_dot_dir1 - dir1_dot_offset;

    // clamp capsule
    term0 = {
        let term_bound_0 = f32x4::clamp(dir0_dot_offset, -length0, *length0);
        let term_bound_1 = f32x4::clamp(
            dir0_dot_offset + length1 * dir0_dot_dir1,
            -length0,
            *length0,
        );
        let term_min = f32x4::min(term_bound_0, term_bound_1);
        let term_max = f32x4::max(term_bound_0, term_bound_1);
        f32x4::clamp(term0, term_min, term_max)
    };

    // clamp edge
    let half_length_dot_offset = length0 * dir0_dot_dir1.absf();
    let edge_min = f32x4::clamp(
        -half_length_dot_offset - dir1_dot_offset,
        f32x4::ZERO,
        *length1,
    );
    let edge_max = f32x4::clamp(
        half_length_dot_offset - dir1_dot_offset,
        f32x4::ZERO,
        *length1,
    );
    term1 = f32x4::clamp(term1, edge_min, edge_max);

    (term0, term1, edge_min, edge_max)
}

struct EdgeTestResult {
    normal: Vec3x4,
    depth: f32x4,
    term_capsule: f32x4,
    term_edge: f32x4,
    edge_min: f32x4,
    edge_max: f32x4,
    edge_direction: Vec3x4,
}

impl EdgeTestResult {
    #[inline]
    pub fn select(&mut self, other: &Self) -> bool32x4 {
        let use_candidate = other.depth.lt(self.depth);
        self.normal = Vec3x4::lane_select(use_candidate, other.normal, self.normal);
        self.edge_direction =
            Vec3x4::lane_select(use_candidate, other.edge_direction, self.edge_direction);
        self.depth = other.depth.select(use_candidate, self.depth);
        self.term_capsule = other.term_capsule.select(use_candidate, self.term_capsule);
        self.term_edge = other.term_edge.select(use_candidate, self.term_edge);
        self.edge_min = other.edge_min.select(use_candidate, self.edge_min);
        self.edge_max = other.edge_max.select(use_candidate, self.edge_max);
        use_candidate
    }
}

/// refer from BEPU
/// * Calculate normal using the closest points of edge to the capsule's axis.
/// * Use this normal to calculate the depth generated by the three edges, and select the smallest.
#[inline]
fn test_edge(
    triangle: &TriangleWide,
    triangle_normal: &Vec3x4, // for fallback
    edge_start: &Vec3x4,
    edge_offset: &Vec3x4,
    capsule_center: &Vec3x4,
    capsule_axis: &Vec3x4,
    capsule_half_height: &f32x4,
) -> EdgeTestResult {
    let edge_length = edge_offset.length();
    let edge_direction = edge_offset * edge_length.recip();
    let (term_capsule, term_edge, edge_min, edge_max) = closest_points(
        capsule_center,
        capsule_axis,
        capsule_half_height,
        edge_start,
        &edge_direction,
        &edge_length,
    );

    let closest_point_on_capsule = capsule_center + capsule_axis * term_capsule;
    let closest_point_on_edge = edge_start + edge_direction * term_edge;
    let mut normal = closest_point_on_capsule - closest_point_on_edge;

    // fallback

    //In the event that the normal has zero length due to the capsule internal line segment
    // touching the edge, use the cross product of the edge and axis.
    let mut normal_squared_length = normal.length_squared();
    // This threshold may need to be explored
    let use_cross = normal_squared_length.lt(f32x4::splat(1e-13));
    let mut cross_normal = Vec3x4::cross(*capsule_axis, edge_offset);
    let calibration_dot = Vec3x4::dot(cross_normal, capsule_center);
    cross_normal =
        Vec3x4::lane_select(calibration_dot.lt(f32x4::ZERO), -cross_normal, cross_normal);
    normal = Vec3x4::lane_select(use_cross, cross_normal, normal);

    //Unfortunately, if the edge and axis are parallel, the cross product will ALSO be zero, so we
    // need another fallback. We'll use the edge plane normal. Unless the triangle is
    // degenerate, this can't be zero length.
    normal_squared_length = normal.length_squared();
    // This threshold may need to be explored
    let use_edge_normal = normal_squared_length.lt(f32x4::splat(1e-13));
    let edge_normal = Vec3x4::cross(*triangle_normal, edge_offset);
    normal = Vec3x4::lane_select(use_edge_normal, edge_normal, normal);

    // 'normalize' may cause panic
    // normal = normal.normalize();
    normal = normal.normalize();

    let edge_dot_normal = {
        let normal_dot_a = Vec3x4::dot(triangle.a.as_vec3x4(), normal);
        let normal_dot_b = Vec3x4::dot(triangle.b.as_vec3x4(), normal);
        let normal_dot_c = Vec3x4::dot(triangle.c.as_vec3x4(), normal);
        f32x4::max(normal_dot_a, f32x4::max(normal_dot_b, normal_dot_c))
    };
    let capsule_offset_dot_normal = Vec3x4::dot(*capsule_center, normal);
    let normal_dot_axis = Vec3x4::dot(normal, *capsule_axis);
    let half_height_on_normal = capsule_half_height * normal_dot_axis;
    let depth = edge_dot_normal - (capsule_offset_dot_normal - half_height_on_normal.absf());

    EdgeTestResult {
        normal,
        depth,
        term_capsule,
        term_edge,
        edge_min,
        edge_max,
        edge_direction,
    }
}

#[inline]
fn clip_against_edge_plane(
    edge_start: &Vec3x4,
    edge_offset: &Vec3x4,
    face_normal: &Vec3x4,
    capsule_center: &Vec3x4,
    capsule_axis: &Vec3x4,
) -> (f32x4, f32x4) {
    let edge_plane_normal = Vec3x4::cross(*face_normal, edge_offset);
    let edge_to_capsule = capsule_center - edge_start;
    let distance = Vec3x4::dot(edge_to_capsule, edge_plane_normal);
    let velocity = Vec3x4::dot(*capsule_axis, edge_plane_normal);
    let velocity_positive = velocity.gt(f32x4::ZERO);
    let t =
        (-distance).select(velocity_positive, distance) / velocity.absf().max(f32x4::splat(1e-15));
    let entry = f32x4::splat(f32::MIN).select(velocity_positive, t);
    let exit = t.select(velocity_positive, f32x4::splat(f32::MAX));
    (entry, exit)
}

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

    // 2 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        capsule: &CapsuleWide,
        local_triangle: &TriangleWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        const LOWER_THRESHOLD_ANGLE: f32 = 0.01;
        const UPPER_THRESHOLD_ANGLE: f32 = 0.05;
        const LOWER_THRESHOLD: f32 = LOWER_THRESHOLD_ANGLE * LOWER_THRESHOLD_ANGLE;
        const UPPER_THRESHOLD: f32 = UPPER_THRESHOLD_ANGLE * UPPER_THRESHOLD_ANGLE;

        // 'local' refers to the space where the triangle is located
        let local_rot_to_world = Mat3x4::from_quat(*contact_context.orientation_b);
        let world_rot_to_local = local_rot_to_world.transpose();
        let local_triangle_center = (local_triangle.a.as_vec3x4()
            + local_triangle.b.as_vec3x4()
            + local_triangle.c.as_vec3x4())
            * f32x4::splat(1.0 / 3.0);

        // Most of the following operations are based on a 'work space' with the origin at the
        // center of the triangle.
        let offset_triangle =
            world_rot_to_local.mul_vec3(*contact_context.offset_b) + local_triangle_center;
        let capsule_center = -offset_triangle;
        let triangle = TriangleWide {
            a: local_triangle.a - local_triangle_center,
            b: local_triangle.b - local_triangle_center,
            c: local_triangle.c - local_triangle_center,
        };

        let capsule_axis =
            world_rot_to_local.mul_vec3(Mat3x4::from_quat(*contact_context.orientation_a).y_axis);

        let ab = triangle.b - triangle.a;
        let ac = triangle.c - triangle.a;
        let bc = triangle.c - triangle.b;
        let ac_x_ab = Vec3x4::cross(ac, ab);
        let face_normal_length = ac_x_ab.length();
        let face_normal = ac_x_ab * face_normal_length.recip();

        let normal_dot_axis = Vec3x4::dot(face_normal, capsule_axis);
        let capsule_center_to_face = Vec3x4::dot(face_normal, capsule_center);

        let face_depth = capsule.half_height * normal_dot_axis.absf() - capsule_center_to_face;

        let (
            edge_normal,
            edge_depth,
            term_capsule,
            term_edge,
            mut edge_min,
            mut edge_max,
            edge_direction,
            edge_start,
        ) = {
            let mut edge_test_result = test_edge(
                &triangle,
                &face_normal,
                &triangle.a.as_vec3x4(),
                &ab,
                &capsule_center,
                &capsule_axis,
                &capsule.half_height,
            );

            let edge_test_result_1 = test_edge(
                &triangle,
                &face_normal,
                &triangle.a.as_vec3x4(),
                &ac,
                &capsule_center,
                &capsule_axis,
                &capsule.half_height,
            );

            edge_test_result.select(&edge_test_result_1);

            let edge_test_result_2 = test_edge(
                &triangle,
                &face_normal,
                &triangle.b.as_vec3x4(),
                &bc,
                &capsule_center,
                &capsule_axis,
                &capsule.half_height,
            );

            let use_b = edge_test_result.select(&edge_test_result_2);

            let edge_start = Point3x4::lane_select(use_b, triangle.b, triangle.a).as_vec3x4();

            (
                edge_test_result.normal,
                edge_test_result.depth,
                edge_test_result.term_capsule,
                edge_test_result.term_edge,
                edge_test_result.edge_min,
                edge_test_result.edge_max,
                edge_test_result.edge_direction,
                edge_start,
            )
        };

        let use_edge_depth = edge_depth.lt(face_depth);
        let depth = edge_depth.select(use_edge_depth, face_depth);
        let normal = Vec3x4::lane_select(use_edge_depth, edge_normal, face_normal);

        let local_normal_dot_face_normal = Vec3x4::dot(normal, face_normal);
        let solid_face_mask = local_normal_dot_face_normal.gt(TriangleWide::BACKFACE_THRESHOLD);

        let allow_contacts = (face_depth + capsule.radius)
            .ge(-contact_context.speculative_margin)
            .bitand(solid_face_mask);

        if allow_contacts == bool32x4::FALSE {
            manifold.reset(2);
            return;
        }

        let mut contact_count = i32x4::ZERO;
        let mut contact0 = Vec3x4::ZERO;
        let mut contact1 = Vec3x4::ZERO;
        if use_edge_depth.any() {
            let plane_normal = Vec3x4::cross(edge_normal, edge_direction);
            let plane_normal_length = plane_normal.length_squared();
            let use_zero = plane_normal_length.lt(f32x4::splat(1e-10));
            let axis_dot_plane_normal = Vec3x4::dot(capsule_axis, plane_normal);
            let squared_angle = f32x4::ZERO.select(
                use_zero,
                axis_dot_plane_normal * axis_dot_plane_normal / plane_normal_length,
            );

            let unclamped_weight = (f32x4::splat(UPPER_THRESHOLD) - squared_angle)
                * f32x4::splat((UPPER_THRESHOLD - LOWER_THRESHOLD).recip());
            let interval_weight = f32x4::clamp(unclamped_weight, f32x4::ZERO, f32x4::ONE);

            let weight = term_edge - term_edge * interval_weight;
            edge_min = interval_weight * edge_min + weight;
            edge_max = interval_weight * edge_max + weight;

            contact0 = edge_direction * edge_min + edge_start;
            contact1 = edge_direction * edge_max + edge_start;

            contact_count = i32x4::splat(2)
                .select(edge_max.gt(edge_min), i32x4::ONE)
                .select(use_edge_depth, i32x4::ZERO);
        }

        //Any edge contributions are now stored in the b0, b1, localNormal, and depth fields.
        // 1) If face contact won (no edges contributed any contacts), then generate two contacts by
        // clipping the capsule axis against the triangle edge planes.
        //(Clipping is used so that the results can be shared by #2, and in case where the capsule
        //(Clipping axis is parallel to the face surface so an edge that 'should' have won, didn't.)
        // 2) If an edge contact won and only generated one contact, then try to generate one
        // additional face contact by projecting a capsule endpoint onto the triangle if
        // it's on the colliding side and is within the triangle's bounds. The bounds check
        // can share the clipping done for face contacts. 3) If an edge contact has
        // generated two contacts, then no additional contacts are required.
        if contact_count.le(i32x4::ONE).bitand(allow_contacts).any() {
            let (ab_entry, ab_exit) = clip_against_edge_plane(
                &triangle.a.as_vec3x4(),
                &ab,
                &face_normal,
                &capsule_center,
                &capsule_axis,
            );
            let (bc_entry, bc_exit) = clip_against_edge_plane(
                &triangle.b.as_vec3x4(),
                &bc,
                &face_normal,
                &capsule_center,
                &capsule_axis,
            );
            let ca = triangle.a - triangle.c;
            let (ca_entry, ca_exit) = clip_against_edge_plane(
                &triangle.c.as_vec3x4(),
                &ca,
                &face_normal,
                &capsule_center,
                &capsule_axis,
            );
            let mut overlap_interval_min = ab_entry
                .max(bc_entry.max(ca_entry))
                .max(-capsule.half_height);
            let mut overlap_interval_max =
                ab_exit.min(bc_exit.min(ca_exit)).min(capsule.half_height);

            let interval_valid_for_second_contact = overlap_interval_max.ge(overlap_interval_min);

            overlap_interval_min = overlap_interval_min.min(capsule.half_height);
            overlap_interval_max = overlap_interval_max.max(-capsule.half_height);

            let clip_on_a0 = capsule_center + capsule_axis * overlap_interval_min;
            let distacne_along_normal_a0 = Vec3x4::dot(clip_on_a0, face_normal);
            let to_remove_a0 = distacne_along_normal_a0 * face_normal;
            let face_candidate_0 = clip_on_a0 - to_remove_a0;

            let clip_on_a1 = capsule_center + capsule_axis * overlap_interval_max;
            let distacne_along_normal_a1 = Vec3x4::dot(clip_on_a1, face_normal);
            let to_remove_a1 = distacne_along_normal_a1 * face_normal;
            let face_candidate_1 = clip_on_a1 - to_remove_a1;

            // if no edge contacts
            {
                let no_edge_contacts = contact_count.eq(i32x4::ZERO);
                let allow_face_contacts = capsule_center_to_face.ge(f32x4::ZERO);
                let use_face_contacts = no_edge_contacts.bitand(allow_face_contacts);
                contact0 = Vec3x4::lane_select(use_face_contacts, face_candidate_0, contact0);
                contact1 = Vec3x4::lane_select(use_face_contacts, face_candidate_1, contact1);
                contact_count = i32x4::splat(2).select(use_face_contacts, contact_count);
            }

            // if there is one edge contact
            {
                let use_face_contact_one = f32x4::absf(overlap_interval_max - term_capsule)
                    .gt(f32x4::absf(overlap_interval_min - term_capsule));
                let face_contact =
                    Vec3x4::lane_select(use_face_contact_one, face_candidate_1, face_candidate_0);
                let face_contact_distance_along_normal =
                    distacne_along_normal_a1.select(use_face_contact_one, distacne_along_normal_a0);
                let use_face_contact_as_second = interval_valid_for_second_contact
                    .bitand(contact_count.eq(i32x4::ONE))
                    .bitand(face_contact_distance_along_normal.gt(f32x4::ZERO));
                contact1 = Vec3x4::lane_select(use_face_contact_as_second, face_contact, contact1);
                contact_count = i32x4::splat(2).select(use_face_contact_as_second, contact_count);
            }
        }

        let capsule_face_normal = Vec3x4::cross(Vec3x4::cross(normal, capsule_axis), capsule_axis);
        let face_normal_dot_normal = Vec3x4::dot(capsule_face_normal, normal);
        let inverse_face_normal_dot_normal = face_normal_dot_normal.recip();
        manifold.depth[0] = Vec3x4::dot(offset_triangle + contact0, capsule_face_normal)
            * inverse_face_normal_dot_normal
            + capsule.radius;
        manifold.depth[1] = Vec3x4::dot(offset_triangle + contact1, capsule_face_normal)
            * inverse_face_normal_dot_normal
            + capsule.radius;

        let collapse = f32x4::absf(face_normal_dot_normal).lt(f32x4::splat(1e-7));

        manifold.depth[0] = (capsule.radius + depth).select(collapse, manifold.depth[0]);
        contact_count = contact_count.select(solid_face_mask, i32x4::ZERO);

        manifold.contact_exists[0] = allow_contacts
            .bitand(contact_count.gt(i32x4::ZERO))
            .bitand(manifold.depth[0].gt(-contact_context.speculative_margin));

        manifold.contact_exists[1] = allow_contacts
            .bitand(contact_count.eq(i32x4::splat(2)))
            .bitand(collapse.not())
            .bitand(manifold.depth[1].gt(-contact_context.speculative_margin));

        //For feature ids, note that we have a few different potential sources of contacts. While
        // we could go through and force each potential source to output ids, there is a
        // useful single unifying factor: where the contacts occur on the capsule axis. Using this,
        // it doesn't matter if contacts are generated from face or edge cases, so there's a
        // higher chance of properly reusing the previous frame's accumulated impulse. Using
        // the depths, push the contacts out toward the capsule, and then compute their projected
        // location along the capsule axis. In other words: ta0 = ((b0 + N * depth0) -
        // capsuleCenter) * capsuleAxis ta1 = ((b1 + N * depth1) - capsuleCenter) *
        // capsuleAxis If ta0 > ta1, featureId0 = 1 and featureId1 = 0, otherwise featureId0
        // = 0 and featureId1 = 1. But note that all we really want is a degree of
        // consistency, so pushing all the way back to the capsule axis isn't necessary.
        // Instead, just compute the projection of the contacts along the capsule axis directly.
        // This will tend to agree with the more in-depth formulation.
        let contact_offset_capsule_0 = contact0 + offset_triangle;
        let contact_offset_capsule_1 = contact1 + offset_triangle;
        let ta0 = Vec3x4::dot(contact_offset_capsule_0, capsule_axis);
        let ta1 = Vec3x4::dot(contact_offset_capsule_1, capsule_axis);
        let flip = ta1.lt(ta0);
        manifold.feature_id[0] = u32x4::ONE.select(flip, u32x4::ZERO);
        manifold.feature_id[1] = u32x4::ZERO.select(flip, u32x4::ONE);

        // The face_flag here has not yet played a role
        let face_flag = u32x4::select(
            u32x4::splat(32768),
            local_normal_dot_face_normal.ge(f32x4::splat(0.999999)),
            u32x4::ZERO,
        );
        manifold.feature_id[0] += face_flag;

        // work space to world space
        manifold.normal = local_rot_to_world
            .mul_vec3(normal)
            .as_unit_vec3x4_unchecked();
        manifold.offset_a[0] = local_rot_to_world.mul_vec3(contact_offset_capsule_0);
        manifold.offset_a[1] = local_rot_to_world.mul_vec3(contact_offset_capsule_1);
    }
}