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

use crate::{
    Candidates, ContactContextTester as ContactContext, ContactManifoldWide,
    Convex4ContactManifoldWide, ConvexContactManifold, CreateShapeWide, Cuboid, CuboidWide,
    ManifoldCandidateWide, PairTest, PairWideTest, ReduceContext, ShapeContainer, ShapeTester,
    ShapeWideTester, Triangle, TriangleWide,
};

struct CuboidFace {
    pub cuboid_face_center: Vec3x4,
    pub cuboid_face_normal: Vec3x4,
    pub cuboid_tangent_x: Vec3x4,
    pub cuboid_tangent_y: Vec3x4,
    pub cuboid_v00: Vec3x4,
    pub cuboid_v01: Vec3x4,
    pub cuboid_v10: Vec3x4,
    pub cuboid_v11: Vec3x4,
    pub feature_id: u32x4,
    pub feature_id_x: u32x4,
    pub feature_id_y: u32x4,
}

struct TriangleFace {
    pub vert_a: Vec3x4,
    pub vert_b: Vec3x4,
    pub vert_c: Vec3x4,
    pub ab: Vec3x4,
    pub bc: Vec3x4,
    pub ca: Vec3x4,
    pub triangle_normal: Vec3x4,
    pub tangent_plane: TangentPlane,
}

struct TangentPlane {
    pub center: Vec3x4,
    pub x: Vec3x4,
    pub y: Vec3x4,
}

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

    // 4 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &CuboidWide,
        b: &TriangleWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        let world_rotation_a = Mat3x4::from_quat(*contact_context.orientation_a);
        let world_rotation_b = Mat3x4::from_quat(*contact_context.orientation_b);

        // The transpose of a rotation matrix is its inverse matrix
        let rotation_b = world_rotation_a.transpose() * world_rotation_b;
        let local_offset_b = world_rotation_a.transpose() * contact_context.offset_b;

        let vert_a = rotation_b * b.a.as_vec3x4() + local_offset_b;
        let vert_b = rotation_b * b.b.as_vec3x4() + local_offset_b;
        let vert_c = rotation_b * b.c.as_vec3x4() + local_offset_b;

        let local_triangle_center = (vert_a + vert_b + vert_c) * f32x4::splat(1.0 / 3.0);
        let ab = vert_b - vert_a;
        let bc = vert_c - vert_b;
        let ca = vert_a - vert_c;

        //left-hand coordinate
        let ab_x_ca = Vec3x4::cross(ab, ca);
        let triangle_normal = ab_x_ca * ab_x_ca.length_recip();

        // Construct the tangent-x and tangent-y on the triangle face.
        let triangle_tangent_x = ab * ab.length_recip();
        let triangle_tangent_y = Vec3x4::cross(triangle_tangent_x, triangle_normal);
        let triangle_face = TriangleFace {
            vert_a,
            vert_b,
            vert_c,
            ab,
            bc,
            ca,
            triangle_normal,
            tangent_plane: TangentPlane {
                center: local_triangle_center,
                x: triangle_tangent_x,
                y: triangle_tangent_y,
            },
        };

        //----------3 x 3 edge cross test--------------
        let (mut depth, mut local_normal) = cross_product_sat_test(a, &triangle_face);
        //----------3 cuboid face normal test----------------
        let (depth_candidate, normal_candidate) = cuboid_face_normal_sat_test(a, &triangle_face);
        //----------triangle face normal SAT test----------------
        let triangle_plane_offset = Vec3x4::dot(triangle_normal, local_triangle_center);
        let negated_triangle_normal = -triangle_normal;
        let triangle_face_depth = f32x4::absf(triangle_normal.x) * a.half_length.x
            + f32x4::absf(triangle_normal.y) * a.half_length.y
            + f32x4::absf(triangle_normal.z) * a.half_length.z
            - f32x4::absf(triangle_plane_offset);
        let calibrated_triangle_normal = Vec3x4::lane_select(
            triangle_plane_offset.gt(f32x4::ZERO),
            negated_triangle_normal,
            triangle_normal,
        );

        //-------------find minimum depth and normal----------------
        (depth, local_normal) = select(depth, local_normal, depth_candidate, normal_candidate);
        (depth, local_normal) = select(
            depth,
            local_normal,
            triangle_face_depth,
            calibrated_triangle_normal,
        );

        // Early exit
        let inactive_lanes =
            u32x4::splat(contact_context.pair_count as u32).le(u32x4::from([0, 1, 2, 3]));
        let normal_dot = Vec3x4::dot(local_normal, triangle_normal);
        let minimum_depth = -contact_context.speculative_margin;
        let solid_face_mask = normal_dot.ge(TriangleWide::BACKFACE_THRESHOLD);
        let valid_depth = depth.ge(minimum_depth);
        let allow_contacts = valid_depth & solid_face_mask & !inactive_lanes;
        if allow_contacts == bool32x4::FALSE {
            manifold.reset(4);
            return;
        }

        // If SAT failed, try to find contact.
        // Find a cuboid face according to the local normal.
        let cuboid_face = compute_cuboid_face(a, local_normal);

        let mut candidates = Candidates::<6>::default();
        let pair_count = contact_context.pair_count;

        // Try to add candidates from the cuboid face.
        add_cuboid_vertices(
            &cuboid_face,
            &triangle_face,
            local_normal,
            normal_dot,
            allow_contacts,
            pair_count,
            &mut candidates,
        );

        // Try to add candidates from the triangle egdes.
        clip_triangle_edges_against_face(
            &triangle_face,
            &cuboid_face,
            local_normal,
            allow_contacts,
            pair_count,
            &mut candidates,
        );

        // Try to reduce candidates
        let b_center_to_a_center = cuboid_face.cuboid_face_center - local_triangle_center;
        let epsilon_scale = f32x4::max(
            a.half_length.x,
            f32x4::max(a.half_length.y, a.half_length.z),
        );
        let contact_normal = local_normal.as_unit_vec3x4_unchecked();
        let reduce_contex = ReduceContext {
            face_a_normal: &cuboid_face.cuboid_face_normal,
            b_center_to_a_center: &b_center_to_a_center,
            face_b_tangent_x: &triangle_tangent_x,
            face_b_tangent_y: &triangle_tangent_y,
        };

        candidates.reduce(
            &reduce_contex,
            minimum_depth,
            epsilon_scale,
            pair_count,
            &contact_normal,
            &mut manifold.contact_exists,
        );

        // Transform data from work space to world space.
        let world_triangle_center = world_rotation_a * local_triangle_center;
        let world_triangle_tangent_x = world_rotation_a * triangle_tangent_x;
        let world_triangle_tangent_y = world_rotation_a * triangle_tangent_y;
        manifold.normal = (world_rotation_a * local_normal).as_unit_vec3x4_unchecked();

        // Transform contact. The result is on triangle temporarily.
        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_contact_to_manifold(
                candidate,
                world_triangle_center,
                world_triangle_tangent_x,
                world_triangle_tangent_y,
                offset_a,
                depth,
                feature_id,
            );
        }

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

fn cross_product_sat_test(a: &CuboidWide, triangle_face: &TriangleFace) -> (f32x4, Vec3x4) {
    // ab check
    let (ab_depth, ab_normal) = test_cuboid_edges_against_triangle_edge(
        a,
        triangle_face.ab,
        triangle_face.tangent_plane.center,
        triangle_face.vert_a,
        triangle_face.vert_b,
        triangle_face.vert_c,
    );

    // bc check
    let (bc_depth, bc_normal) = test_cuboid_edges_against_triangle_edge(
        a,
        triangle_face.bc,
        triangle_face.tangent_plane.center,
        triangle_face.vert_a,
        triangle_face.vert_b,
        triangle_face.vert_c,
    );

    // ca check
    let (ca_depth, ca_normal) = test_cuboid_edges_against_triangle_edge(
        a,
        triangle_face.ca,
        triangle_face.tangent_plane.center,
        triangle_face.vert_a,
        triangle_face.vert_b,
        triangle_face.vert_c,
    );

    let (depth, local_normal) = select(ab_depth, ab_normal, bc_depth, bc_normal);
    select(depth, local_normal, ca_depth, ca_normal)
}

fn cuboid_face_normal_sat_test(a: &CuboidWide, triangle_face: &TriangleFace) -> (f32x4, Vec3x4) {
    let x_normal_sign = f32x4::select(
        f32x4::ONE,
        triangle_face.tangent_plane.center.x.lt(f32x4::ZERO),
        f32x4::NEG_ONE,
    );
    let y_normal_sign = f32x4::select(
        f32x4::ONE,
        triangle_face.tangent_plane.center.y.lt(f32x4::ZERO),
        f32x4::NEG_ONE,
    );
    let z_normal_sign = f32x4::select(
        f32x4::ONE,
        triangle_face.tangent_plane.center.z.lt(f32x4::ZERO),
        f32x4::NEG_ONE,
    );

    let x_depth = get_depth_for_interval(
        a.half_length.x,
        triangle_face.vert_a.x,
        triangle_face.vert_b.x,
        triangle_face.vert_c.x,
    );
    let y_depth = get_depth_for_interval(
        a.half_length.y,
        triangle_face.vert_a.y,
        triangle_face.vert_b.y,
        triangle_face.vert_c.y,
    );
    let z_depth = get_depth_for_interval(
        a.half_length.z,
        triangle_face.vert_a.z,
        triangle_face.vert_b.z,
        triangle_face.vert_c.z,
    );
    let (depth, local_normal) = select(
        x_depth,
        Vec3x4::new(x_normal_sign, f32x4::ZERO, f32x4::ZERO),
        y_depth,
        Vec3x4::new(f32x4::ZERO, y_normal_sign, f32x4::ZERO),
    );
    select(
        depth,
        local_normal,
        z_depth,
        Vec3x4::new(f32x4::ZERO, f32x4::ZERO, z_normal_sign),
    )
}

fn select(
    depth: f32x4,
    local_normal: Vec3x4,
    depth_candidate: f32x4,
    local_normal_candidate: Vec3x4,
) -> (f32x4, Vec3x4) {
    (
        f32x4::min(depth, depth_candidate),
        Vec3x4::lane_select(
            depth_candidate.lt(depth),
            local_normal_candidate,
            local_normal,
        ),
    )
}

#[allow(clippy::too_many_arguments)]
fn check_triangle_contains_point(
    vert_a: Vec3x4,
    vert_b: Vec3x4,
    vertex: Vec3x4,
    triangle_normal: Vec3x4,
    contact_normal: Vec3x4,
    inverse_normal_dot: f32x4,
    ab_edge_normal: Vec3x4,
    bc_edge_normal: Vec3x4,
    ca_edge_normal: Vec3x4,
) -> (Vec3x4, bool32x4) {
    // We first project the point onto the plane and then use the point on the plane to
    // determine whether it is inside the triangle.
    // Of course, there may be other ways to do it.
    let av = vertex - vert_a;
    let plane_distance = Vec3x4::dot(triangle_normal, av);
    let offset = contact_normal * plane_distance * inverse_normal_dot;
    let v_on_plane = vertex - offset;

    let a_to_v = v_on_plane - vert_a;
    let b_to_v = v_on_plane - vert_b;
    let ab_dot = Vec3x4::dot(a_to_v, ab_edge_normal);
    let bc_dot = Vec3x4::dot(b_to_v, bc_edge_normal);
    let ca_dot = Vec3x4::dot(a_to_v, ca_edge_normal);

    // Note that plane normals are assumed to point inward.
    let contained = ab_dot.ge(f32x4::ZERO) & bc_dot.ge(f32x4::ZERO) & ca_dot.ge(f32x4::ZERO);
    (v_on_plane, contained)
}

fn add_candidate(
    v_on_plane: Vec3x4,
    tangent_plane: &TangentPlane,
    feature_id: u32x4,
    exists: bool32x4,
    pair_count: usize,
    candidates: &mut Candidates<6>,
) {
    let offset = v_on_plane - tangent_plane.center;
    let x = Vec3x4::dot(offset, tangent_plane.x);
    let y = Vec3x4::dot(offset, tangent_plane.y);
    let candidate = ManifoldCandidateWide {
        x,
        y,
        depth: f32x4::ZERO,
        feature_id,
    };

    candidates.add(&candidate, exists, pair_count);
}

fn get_depth_for_interval(extreme: f32x4, a: f32x4, b: f32x4, c: f32x4) -> f32x4 {
    let min = f32x4::min(a, f32x4::min(b, c));
    let max = f32x4::max(a, f32x4::max(b, c));

    f32x4::min(extreme - min, max + extreme)
}

#[allow(clippy::too_many_arguments)]
fn test_face_normal_against_edge(
    edge_y: f32x4,
    edge_z: f32x4,
    triangle_center_y: f32x4,
    triangle_center_z: f32x4,
    edge_y2: f32x4,
    edge_z2: f32x4,
    half_height: f32x4,
    half_length: f32x4,
    vert_a_y: f32x4,
    vert_a_z: f32x4,
    vert_b_y: f32x4,
    vert_b_z: f32x4,
    vert_c_y: f32x4,
    vert_c_z: f32x4,
) -> (f32x4, f32x4, f32x4, f32x4) {
    // Pay attention that this method assumes the x-axis as the face normal for calculations.
    // When used externally, you need to swap the coordinates yourself to meet the assumption.

    // In this case, the simplified notation of the cross product.
    let local_normal_x = f32x4::ZERO;
    let mut local_normal_y = edge_z;
    let mut local_normal_z = -edge_y;

    let calibration_dot = triangle_center_y * local_normal_y + triangle_center_z * local_normal_z;
    let length = (edge_y2 + edge_z2).sqrtf();
    // Determine whether the triangle is above or below the cuboid, and perform normal calibration
    // based on the different cases
    let inverse_length =
        f32x4::select(f32x4::ONE, calibration_dot.lt(f32x4::ZERO), f32x4::NEG_ONE) * length.recip();

    local_normal_y *= inverse_length;
    local_normal_z *= inverse_length;

    // Calculate the extreme dot value of the cuboid.
    let extreme =
        f32x4::absf(local_normal_y) * half_height + f32x4::absf(local_normal_z) * half_length;

    // Calculate the 3 triangle vertex dot value.
    let projection_a = vert_a_y * local_normal_y + vert_a_z * local_normal_z;
    let projection_b = vert_b_y * local_normal_y + vert_b_z * local_normal_z;
    let projection_c = vert_c_y * local_normal_y + vert_c_z * local_normal_z;

    // Calculate the intersection depth of the two projected line segments.
    let mut depth = get_depth_for_interval(extreme, projection_a, projection_b, projection_c);
    depth = f32x4::select(f32x4::MAX, length.lt(f32x4::splat(1e-7)), depth);
    (depth, local_normal_x, local_normal_y, local_normal_z)
}

fn test_cuboid_edges_against_triangle_edge(
    a: &CuboidWide,
    triangle_egde_offset: Vec3x4,
    triangle_center: Vec3x4,
    vert_a: Vec3x4,
    vert_b: Vec3x4,
    vert_c: Vec3x4,
) -> (f32x4, Vec3x4) {
    let x2 = triangle_egde_offset.x * triangle_egde_offset.x;
    let y2 = triangle_egde_offset.y * triangle_egde_offset.y;
    let z2 = triangle_egde_offset.z * triangle_egde_offset.z;

    // Using the x-direction face normal, calculate the cross product of the triangle edge.
    let (mut depth, local_normal_x, local_normal_y, local_normal_z) = test_face_normal_against_edge(
        triangle_egde_offset.y,
        triangle_egde_offset.z,
        triangle_center.y,
        triangle_center.z,
        y2,
        z2,
        a.half_length.y,
        a.half_length.z,
        vert_a.y,
        vert_a.z,
        vert_b.y,
        vert_b.z,
        vert_c.y,
        vert_c.z,
    );

    // Using the y-direction face normal, calculate the cross product of the triangle edge.
    let (
        depth_candidate,
        local_normal_candidate_y,
        local_normal_candidate_x,
        local_normal_candidate_z,
    ) = test_face_normal_against_edge(
        triangle_egde_offset.x,
        triangle_egde_offset.z,
        triangle_center.x,
        triangle_center.z,
        x2,
        z2,
        a.half_length.x,
        a.half_length.z,
        vert_a.x,
        vert_a.z,
        vert_b.x,
        vert_b.z,
        vert_c.x,
        vert_c.z,
    );

    let mut local_normal = Vec3x4::new(local_normal_x, local_normal_y, local_normal_z);
    let local_normal_candidate = Vec3x4::new(
        local_normal_candidate_x,
        local_normal_candidate_y,
        local_normal_candidate_z,
    );

    local_normal = Vec3x4::lane_select(
        depth_candidate.lt(depth),
        local_normal_candidate,
        local_normal,
    );
    depth = f32x4::min(depth, depth_candidate);

    // Using the z-direction face normal, calculate the cross product of the triangle edge.
    let (
        depth_candidate,
        local_normal_candidate_z,
        local_normal_candidate_x,
        local_normal_candidate_y,
    ) = test_face_normal_against_edge(
        triangle_egde_offset.x,
        triangle_egde_offset.y,
        triangle_center.x,
        triangle_center.y,
        x2,
        y2,
        a.half_length.x,
        a.half_length.y,
        vert_a.x,
        vert_a.y,
        vert_b.x,
        vert_b.y,
        vert_c.x,
        vert_c.y,
    );

    let local_normal_candidate = Vec3x4::new(
        local_normal_candidate_x,
        local_normal_candidate_y,
        local_normal_candidate_z,
    );

    local_normal = Vec3x4::lane_select(
        depth_candidate.lt(depth),
        local_normal_candidate,
        local_normal,
    );
    depth = f32x4::min(depth, depth_candidate);

    (depth, local_normal)
}

fn clip_triangle_edges_against_face(
    triangle_face: &TriangleFace,
    cuboid_face: &CuboidFace,
    contact_normal: Vec3x4,
    allow_contacts: bool32x4,
    pair_count: usize,
    candidates: &mut Candidates<6>,
) {
    // Calculate the tangent plane axis of the contact normal.
    let edge_plane_normal_x = Vec3x4::cross(cuboid_face.cuboid_tangent_y, contact_normal);
    let edge_plane_normal_y = Vec3x4::cross(cuboid_face.cuboid_tangent_x, contact_normal);
    let base_id = u32x4::splat(4);
    let six_wide = u32x4::splat(6);

    let cuboid_v00 = cuboid_face.cuboid_v00;
    let cuboid_v11 = cuboid_face.cuboid_v11;
    for (edge_start, edge_direction, feature_id) in [
        (triangle_face.vert_a, triangle_face.ab, base_id),
        (triangle_face.vert_b, triangle_face.bc, base_id + u32x4::ONE),
        (
            triangle_face.vert_c,
            triangle_face.ca,
            base_id + u32x4::splat(2),
        ),
    ] {
        let (min_location, min_exist, max_location, max_exist) = clip_triangle_edge_against_face(
            edge_start,
            edge_direction,
            cuboid_v00,
            cuboid_v11,
            edge_plane_normal_x,
            edge_plane_normal_y,
        );
        add_candidate(
            min_location,
            &triangle_face.tangent_plane,
            feature_id,
            min_exist & allow_contacts & candidates.count.lt(six_wide),
            pair_count,
            candidates,
        );
        add_candidate(
            max_location,
            &triangle_face.tangent_plane,
            feature_id,
            max_exist & allow_contacts & candidates.count.lt(six_wide),
            pair_count,
            candidates,
        );
    }
}

fn clip_triangle_edge_against_face(
    edge_start: Vec3x4,
    edge_direction: Vec3x4,
    cuboid_v00: Vec3x4,
    cuboid_v11: Vec3x4,
    edge_plane_normal_x: Vec3x4,
    edge_plane_normal_y: Vec3x4,
) -> (Vec3x4, bool32x4, Vec3x4, bool32x4) {
    let edge_start_to_v00 = cuboid_v00 - edge_start;
    let edge_start_to_v11 = cuboid_v11 - edge_start;

    // Check the value in the x-direction of the tangent plane
    let (min_x, max_x) = clip_triangle_edge_against_plane(
        edge_direction,
        edge_start_to_v00,
        edge_start_to_v11,
        edge_plane_normal_x,
    );
    // Check the value in the y-direction of the tangent plane
    let (min_y, max_y) = clip_triangle_edge_against_plane(
        edge_direction,
        edge_start_to_v00,
        edge_start_to_v11,
        edge_plane_normal_y,
    );
    let min = f32x4::max(min_x, min_y);
    let max = f32x4::min(f32x4::ONE, f32x4::min(max_x, max_y));
    let min_localtion = edge_start + edge_direction * min;
    let max_localtion = edge_start + edge_direction * max;

    // Check whether the min, max is valid;
    let min_exist = (max - min).ge(f32x4::splat(1e-5)) & min.lt(f32x4::ONE) & min.gt(f32x4::ZERO);
    let max_exist = max.ge(min) & max.le(f32x4::ONE) & max.ge(f32x4::ZERO);

    (min_localtion, min_exist, max_localtion, max_exist)
}

fn clip_triangle_edge_against_plane(
    edge_direction: Vec3x4,
    edge_start_to_v00: Vec3x4,
    edge_start_to_v11: Vec3x4,
    check_axis: Vec3x4,
) -> (f32x4, f32x4) {
    let distance_0 = Vec3x4::dot(edge_start_to_v00, check_axis);
    let distance_1 = Vec3x4::dot(edge_start_to_v11, check_axis);
    let velocity = Vec3x4::dot(check_axis, edge_direction);

    let edge_start_inside = (distance_0 * distance_1).le(f32x4::ZERO);
    let dont_use_fallback = f32x4::absf(velocity).gt(f32x4::splat(1e-15));
    // Using the principle of similar triangles, calculate the coefficient of the intersection point
    // along the edge direction. The coefficient includes the reciprocal of the edge's magnitude.
    let t0 = distance_0 * velocity.recip();
    let t1 = distance_1 * velocity.recip();

    let min = f32x4::select(
        f32x4::min(t0, t1),
        dont_use_fallback,
        f32x4::select(f32x4::MIN, edge_start_inside, f32x4::MAX),
    );
    let max = f32x4::select(
        f32x4::max(t0, t1),
        dont_use_fallback,
        f32x4::select(f32x4::MAX, edge_start_inside, f32x4::MIN),
    );

    (min, max)
}

fn transform_contact_to_manifold(
    raw_contact: &ManifoldCandidateWide,
    face_center_b: Vec3x4,
    tangent_b_x: Vec3x4,
    tangent_b_y: Vec3x4,
    manifold_offset_a: &mut Vec3x4,
    manifold_depth: &mut f32x4,
    manifold_feature_id: &mut u32x4,
) {
    let x = raw_contact.x * tangent_b_x;
    let y = raw_contact.y * tangent_b_y;
    *manifold_offset_a = x + y + face_center_b;
    *manifold_depth = raw_contact.depth;
    *manifold_feature_id = raw_contact.feature_id;
}

fn add_cuboid_vertices(
    cuboid_face: &CuboidFace,
    triangle_face: &TriangleFace,
    contact_normal: Vec3x4,
    normal_dot: f32x4,
    allow_contacts: bool32x4,
    pair_count: usize,
    candidates: &mut Candidates<6>,
) {
    // Check and add four cuboid face vertices.
    // These vectors are used to determine whether the projection point lies within the
    // triangle.
    // normal_dot is from dot(local_normal, triangle_normal).
    let inverse_normal_dot = f32x4::select(
        normal_dot.recip(),
        f32x4::absf(normal_dot).gt(f32x4::splat(1e-10)),
        f32x4::MAX,
    );
    let ab_edge_normal = Vec3x4::cross(triangle_face.ab, triangle_face.triangle_normal);
    let bc_edge_normal = Vec3x4::cross(triangle_face.bc, triangle_face.triangle_normal);
    let ca_edge_normal = Vec3x4::cross(triangle_face.ca, triangle_face.triangle_normal);

    for (face_vertex, feature_id) in [
        (cuboid_face.cuboid_v00, cuboid_face.feature_id),
        (
            cuboid_face.cuboid_v01,
            cuboid_face.feature_id + cuboid_face.feature_id_y,
        ),
        (
            cuboid_face.cuboid_v10,
            cuboid_face.feature_id + cuboid_face.feature_id_x,
        ),
        (
            cuboid_face.cuboid_v11,
            cuboid_face.feature_id + cuboid_face.feature_id_x + cuboid_face.feature_id_y,
        ),
    ] {
        let (v_on_plane, contained) = check_triangle_contains_point(
            triangle_face.vert_a,
            triangle_face.vert_b,
            face_vertex,
            triangle_face.triangle_normal,
            contact_normal,
            inverse_normal_dot,
            ab_edge_normal,
            bc_edge_normal,
            ca_edge_normal,
        );
        add_candidate(
            v_on_plane,
            &triangle_face.tangent_plane,
            feature_id,
            contained & allow_contacts,
            pair_count,
            candidates,
        );
    }
}

fn compute_cuboid_face(a: &CuboidWide, contact_normal: Vec3x4) -> CuboidFace {
    let abs_nx = f32x4::absf(contact_normal.x);
    let abs_ny = f32x4::absf(contact_normal.y);
    let abs_nz = f32x4::absf(contact_normal.z);

    let x_bigger_than_y = abs_nx.gt(abs_ny);
    let x_bigger_than_z = abs_nx.gt(abs_nz);
    let y_bigger_than_z = abs_ny.gt(abs_nz);

    let use_x = x_bigger_than_y & x_bigger_than_z;
    let use_y = y_bigger_than_z & !use_x;

    let normal_is_negative_x = contact_normal.x.lt(f32x4::ZERO);
    let normal_is_negative_y = contact_normal.y.lt(f32x4::ZERO);
    let normal_is_negative_z = contact_normal.z.lt(f32x4::ZERO);

    // Construct the tangent-x on the cuboid face.
    let cuboid_tangent_x = Vec3x4::lane_select(use_x, Vec3x4::Z, Vec3x4::X);

    // Construct the tangent-y on the cuboid face.
    let cuboid_tangent_y = Vec3x4::lane_select(use_y, Vec3x4::Z, Vec3x4::Y);

    // Construct the face normal of the cuboid face.
    let cuboid_face_normal = Vec3x4::lane_select(
        use_x,
        Vec3x4::lane_select(normal_is_negative_x, Vec3x4::X, Vec3x4::NEG_X),
        Vec3x4::lane_select(
            use_y,
            Vec3x4::lane_select(normal_is_negative_y, Vec3x4::Y, Vec3x4::NEG_Y),
            Vec3x4::lane_select(normal_is_negative_z, Vec3x4::Z, Vec3x4::NEG_Z),
        ),
    );

    let half_extent_x = f32x4::select(a.half_length.z, use_x, a.half_length.x);
    let half_extent_y = f32x4::select(a.half_length.z, use_y, a.half_length.y);
    let half_extent_z = f32x4::select(
        a.half_length.x,
        use_x,
        f32x4::select(a.half_length.y, use_y, a.half_length.z),
    );
    let cuboid_face_center = cuboid_face_normal * half_extent_z;

    // Calculate the four vertices on the cuboid face.
    let cuboid_edge_offset_x = cuboid_tangent_x * half_extent_x;
    let cuboid_edge_offset_y = cuboid_tangent_y * half_extent_y;
    let positive_x = cuboid_face_center + cuboid_edge_offset_x;
    let negative_x = cuboid_face_center - cuboid_edge_offset_x;
    let cuboid_v00 = negative_x - cuboid_edge_offset_y;
    let cuboid_v01 = negative_x + cuboid_edge_offset_y;
    let cuboid_v10 = positive_x - cuboid_edge_offset_y;
    let cuboid_v11 = positive_x + cuboid_edge_offset_y;

    // Setup feature id.
    let local_x_id = u32x4::ZERO;
    let local_y_id = u32x4::ONE;
    let local_z_id = u32x4::splat(2);
    let axis_id_tangent_x = u32x4::select(local_z_id, use_x, local_x_id);
    let axis_id_tangent_y = u32x4::select(local_z_id, use_y, local_y_id);
    let axis_id_normal = u32x4::select(
        u32x4::select(local_x_id, normal_is_negative_x, u32x4::ZERO),
        use_x,
        u32x4::select(
            u32x4::select(local_y_id, normal_is_negative_y, u32x4::ZERO),
            use_y,
            u32x4::select(local_z_id, normal_is_negative_z, u32x4::ZERO),
        ),
    );

    CuboidFace {
        cuboid_face_center,
        cuboid_face_normal,
        cuboid_tangent_x,
        cuboid_tangent_y,
        cuboid_v00,
        cuboid_v01,
        cuboid_v10,
        cuboid_v11,
        feature_id: axis_id_normal,
        feature_id_x: axis_id_tangent_x,
        feature_id_y: axis_id_tangent_y,
    }
}

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