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::{vec2x4, vec3x4, Cross, Dot, Mat3x4, Vec2x4, Vec3x4};

use super::common::{generate_interior_points, EPS_6};
use super::cuboid_cylinder_tester::intersect_line_circle;
use super::cylinder_cylinder_tester::project_onto_cap_b;
use super::tootbird::find_minimum_depth;
use crate::{
    Candidates, ContactContextTester as ContactContext, Convex4ContactManifoldWide, CylinderWide,
    IterContext, ManifoldCandidateWide, MinkowskiSupportWide, PairWideTest, ReduceContext,
    ShapeWideTester, TransformWide, TriangleWide,
};

impl PairWideTest<TriangleWide, CylinderWide> for ShapeWideTester {
    // note: If this function returns false, it will cause a bug—the tester will continue to use the
    // result from the previous solution, leading to an incorrect manifold output
    #[inline]
    fn should_reset_manifold_before_test() -> bool {
        true
    }

    // note: ContactContext.offset_b means shapeA.worldPos - shapeB.worldPos.
    // Convex4ContactManifoldWide.offset_a means contactPoint.worldPos - shapeA.worldPos.
    // In contrast, the 'offset_b' in manifold refers to contactPoint.worldPos - shapeB.worldPos;
    // We use manifold.offset_a to store intermediate offset_b values during computation. and
    // finally, transform them.
    // 4 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &TriangleWide,
        b: &CylinderWide,
        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);

        // Work in b's local space.
        // The transpose of a rotation matrix is its inverse matrix
        let rotation_a = world_rotation_b.transpose() * world_rotation_a;
        let local_offset_b = world_rotation_b.transpose() * contact_context.offset_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;
        let length = local_triangle_center.length();
        let mut initial_normal = local_triangle_center * length.recip();
        // Set up initial normal.
        let use_initial_sample_fallback = length.lt(f32x4::splat(1e-10f32));
        initial_normal.x =
            f32x4::select(f32x4::ZERO, use_initial_sample_fallback, initial_normal.x);
        initial_normal.y = f32x4::select(f32x4::ONE, use_initial_sample_fallback, initial_normal.y);
        initial_normal.z =
            f32x4::select(f32x4::ZERO, use_initial_sample_fallback, initial_normal.z);

        // 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 cylinder's position is within the triangle and below the triangle plane. If
        // so, we can ignore it.
        let cylinder_to_triangle_dot = Vec3x4::dot(triangle_normal, local_triangle_center);
        let cylinder_below_plane = cylinder_to_triangle_dot.gt(f32x4::ZERO);

        // Inward egde normal
        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 cylinder_inside_triangle_edge_planes = ab_plane_test.le(f32x4::ZERO)
            & bc_plane_test.le(f32x4::ZERO)
            & ca_plane_test.le(f32x4::ZERO);
        let cylinder_inside_and_below_triangle =
            cylinder_inside_triangle_edge_planes & cylinder_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 | cylinder_inside_and_below_triangle;

        //ignore degenerate triangle check.
        if inactive_lanes.all() {
            manifold.reset(4);
            return;
        }

        //First attempt to locate a support point.
        let negated_triangle_normal = -triangle_normal;
        let cylinder_support_along_negated_triangle_normal = b
            .support_point_local(negated_triangle_normal, None)
            .point
            .as_vec3x4();
        let negated_triangle_normal_support =
            cylinder_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 cylinder 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 - cylinder_support_along_negated_triangle_normal;
        let closest_to_b = triangle_b - cylinder_support_along_negated_triangle_normal;
        let closest_to_c = triangle_c - cylinder_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 = !cylinder_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 epsilon_scale = f32x4::max(b.half_height, b.radius);
        //Get proper local normal and depth.
        let (local_normal, closest_on_b, depth) = {
            if skip_depth_refine.all() {
                (
                    negated_triangle_normal,
                    cylinder_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,
                        epsilon_scale * EPS_6,
                        depth_threshold,
                        25,
                        None,
                        false,
                    ),
                );

                (
                    Vec3x4::lane_select(
                        skip_depth_refine,
                        negated_triangle_normal,
                        toot_bird_result.normal.as_vec3x4(),
                    ),
                    Vec3x4::lane_select(
                        skip_depth_refine,
                        cylinder_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 face_normal_a_dot_normal = Vec3x4::dot(triangle_normal, local_normal);

        //If the cylinder 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)
            | face_normal_a_dot_normal.gt(TriangleWide::BACKFACE_THRESHOLD);

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

        //Now, We have a local_normal and a closest_on_b, try to find 'contact point'
        let use_triangle_edge_case =
            f32x4::absf(face_normal_a_dot_normal).lt(f32x4::splat(0.2f32)) & !inactive_lanes;
        let cap_center_b_y = f32x4::select(
            -b.half_height,
            local_normal.y.lt(f32x4::ZERO),
            b.half_height,
        );

        //0.70710678118 is cos45°.
        let use_cap =
            f32x4::absf(local_normal.y).gt(f32x4::splat(0.707_106_77_f32)) & !inactive_lanes;
        let contact_normal = local_normal.as_unit_vec3x4_unchecked();

        // Cylinder cap tests
        if use_cap.any() {
            let mut candidates = Candidates::<10>::default();
            let pair_count = contact_context.pair_count;

            let inverse_contact_normal_y = local_normal.y.recip();

            //Firstly, check the three triangle edges. Project them to cylinder cap.
            let projected_a = project_onto_cap_b(
                cap_center_b_y,
                inverse_contact_normal_y,
                contact_normal,
                triangle_a,
            );
            let projected_b = project_onto_cap_b(
                cap_center_b_y,
                inverse_contact_normal_y,
                contact_normal,
                triangle_b,
            );
            let projected_c = project_onto_cap_b(
                cap_center_b_y,
                inverse_contact_normal_y,
                contact_normal,
                triangle_c,
            );

            let projected_ab = projected_b - projected_a;
            let projected_bc = projected_c - projected_b;
            let projected_ca = projected_a - projected_c;

            //Construct triangle tangent space.
            let triangle_ab_length = triangle_ab.length();
            let triangle_tangent_x = triangle_ab * triangle_ab_length.recip();
            let triangle_tangent_y = Vec3x4::cross(triangle_tangent_x, triangle_normal);

            let tangent_a = Vec2x4::new(
                Vec3x4::dot(centroid_a, triangle_tangent_x),
                Vec3x4::dot(centroid_a, triangle_tangent_y),
            );
            let tangent_b = Vec2x4::new(
                Vec3x4::dot(centroid_b, triangle_tangent_x),
                Vec3x4::dot(centroid_b, triangle_tangent_y),
            );
            let tangent_c = Vec2x4::new(
                Vec3x4::dot(centroid_c, triangle_tangent_x),
                Vec3x4::dot(centroid_c, triangle_tangent_y),
            );

            let tangent_ab = tangent_b - tangent_a;
            let tangent_bc = tangent_c - tangent_b;
            let tangent_ca = tangent_a - tangent_c;

            for (projected_point, projected_offset, tangent_point, tangent_offset, edge_id) in [
                (
                    projected_a,
                    projected_ab,
                    tangent_a,
                    tangent_ab,
                    u32x4::ZERO,
                ),
                (projected_b, projected_bc, tangent_b, tangent_bc, u32x4::ONE),
                (projected_c, projected_ca, tangent_c, tangent_ca, u32x4::TWO),
            ] {
                //Check intersection
                let (mut t_min, mut t_max, intersected) =
                    intersect_line_circle(projected_point, projected_offset, b.radius);

                //Clamp 01
                t_min = t_min.clamp(f32x4::ZERO, f32x4::ONE);
                t_max = t_max.clamp(f32x4::ZERO, f32x4::ONE);

                //Try add edge
                try_add_edge(
                    &EdgeInterectionInfo {
                        edge_start: tangent_point,
                        edge_offset: tangent_offset,
                        t_min,
                        t_max,
                    },
                    intersected & use_cap,
                    edge_id,
                    pair_count,
                    &mut candidates,
                );
            }

            //Consider triangle face contact?
            let use_cap_triangle_face = use_cap & !use_triangle_edge_case;

            if use_cap_triangle_face.any() {
                //Create cylinder interior point on its cap.
                let (
                    interior_on_cylinder0,
                    interior_on_cylinder1,
                    interior_on_cylinder2,
                    interior_on_cylinder3,
                ) = generate_interior_points(b, local_normal, closest_on_b);

                let inverse_denominator = face_normal_a_dot_normal.recip();
                let y_offset = local_triangle_center.y - cap_center_b_y;

                let tangent_local_normal = Vec2x4::new(
                    Vec3x4::dot(local_normal, triangle_tangent_x),
                    Vec3x4::dot(local_normal, triangle_tangent_y),
                );

                //Calculate interior.y on triangle tangent space.
                let y_on_tangent_x = y_offset * triangle_tangent_x.y;
                let y_on_tangent_y = y_offset * triangle_tangent_y.y;

                let tangent_info = TangentInfo {
                    tangent_a,
                    tangent_ab,
                    tangent_b,
                    tangent_bc,
                    tangent_c,
                    tangent_ca,
                };

                for (interoir, feature_id) in [
                    (interior_on_cylinder0, u32x4::splat(8)),
                    (interior_on_cylinder1, u32x4::splat(9)),
                    (interior_on_cylinder2, u32x4::splat(10)),
                    (interior_on_cylinder3, u32x4::splat(11)),
                ] {
                    //Project interior to triangle face.
                    let x_offset = local_triangle_center.x - interoir.x;
                    let z_offset = local_triangle_center.z - interoir.y;
                    let t = Vec3x4::dot(local_normal, vec3x4(x_offset, y_offset, z_offset))
                        * inverse_denominator;

                    //According t, calculate projected interoir in triangle tangent space.
                    let point = vec2x4(
                        tangent_local_normal.x * t
                            - x_offset * triangle_tangent_x.x
                            - y_on_tangent_x
                            - z_offset * triangle_tangent_x.z,
                        tangent_local_normal.y * t
                            - x_offset * triangle_tangent_y.x
                            - y_on_tangent_y
                            - z_offset * triangle_tangent_y.z,
                    );

                    //Try to add interior points, with only those points in triangle.
                    try_add_interior_point(
                        point,
                        feature_id,
                        &tangent_info,
                        use_cap_triangle_face,
                        pair_count,
                        &mut candidates,
                    );
                }
            }

            let cap_normal =
                Vec3x4::lane_select(local_normal.y.lt(f32x4::ZERO), Vec3x4::Y, Vec3x4::NEG_Y);

            let triangle_center_to_cap_center = vec3x4(
                -local_triangle_center.x,
                cap_center_b_y - local_triangle_center.y,
                -local_triangle_center.z,
            );

            let reduce_contex = ReduceContext {
                face_a_normal: &-cap_normal,
                b_center_to_a_center: &triangle_center_to_cap_center,
                face_b_tangent_x: &triangle_tangent_x,
                face_b_tangent_y: &triangle_tangent_y,
            };

            //Here, shape A contributes the vector for the tangent plane, so the normal needs to be
            // inverted.
            candidates.reduce(
                &reduce_contex,
                depth_threshold,
                epsilon_scale,
                pair_count,
                &-contact_normal,
                &mut manifold.contact_exists,
            );

            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()),
                ),
            ) {
                //Temporarily, the computed offset_b data is stored in manifold.offset_a.
                *offset_a = triangle_tangent_x * candidate.x
                    + triangle_tangent_y * candidate.y
                    + local_triangle_center;
                *feature_id = candidate.feature_id;
                *depth = candidate.depth;
            }
        }

        // Cylinder side tests
        // In this case, at most two contact points may be generated.
        let use_side = !use_cap & !inactive_lanes;
        if use_side.any() {
            let mut cylinder_t_min = f32x4::ZERO;
            let mut cylinder_t_max = f32x4::ZERO;
            let mut depth_t_min = f32x4::ZERO;
            let mut depth_t_max = f32x4::ZERO;

            //Is cylinder side and triangle edge case?
            let use_side_egde_case = use_side & use_triangle_edge_case;

            //Determine the dominant edge of the triangle.
            let ab_edge_alignment = Vec3x4::dot(egde_plane_ab, local_normal);
            let bc_edge_alignment = Vec3x4::dot(egde_plane_bc, local_normal);
            let ca_edge_alignment = Vec3x4::dot(egde_plane_ca, local_normal);

            let max = f32x4::maxf(
                f32x4::maxf(ab_edge_alignment, bc_edge_alignment),
                ca_edge_alignment,
            );

            let ab_is_dominant = max.eq(ab_edge_alignment);
            let bc_is_dominant = max.eq(bc_edge_alignment);

            //Set up dominant edge.
            let dominant_edge_start = Vec3x4::lane_select(
                ab_is_dominant,
                triangle_a,
                Vec3x4::lane_select(bc_is_dominant, triangle_b, triangle_c),
            );
            let dominant_edge_offset = Vec3x4::lane_select(
                ab_is_dominant,
                triangle_ab,
                Vec3x4::lane_select(bc_is_dominant, triangle_bc, triangle_ca),
            );

            //The following code is used to handle cases when the dominant edge of the triangle is
            // nearly parallel to the cylinder side.
            let lower_sin_angle_threshold = f32x4::splat(0.01f32);
            let upper_sin_angle_threshold = f32x4::splat(0.02f32);
            let dominant_edge_dot_horizontal_normal =
                dominant_edge_offset.z * local_normal.x - dominant_edge_offset.x * local_normal.z;
            let dominant_edge_dot_horizontal_normal_squared =
                dominant_edge_dot_horizontal_normal * dominant_edge_dot_horizontal_normal;
            let dominant_edge_length_squared = dominant_edge_offset.length_squared();
            let horizontal_normal_length_squared =
                local_normal.x * local_normal.x + local_normal.z * local_normal.z;
            let interpolation_scale =
                dominant_edge_length_squared * horizontal_normal_length_squared;
            let interpolation_min = lower_sin_angle_threshold * lower_sin_angle_threshold;
            let inverse_interpolation_span = (upper_sin_angle_threshold
                * upper_sin_angle_threshold
                - lower_sin_angle_threshold * lower_sin_angle_threshold)
                .recip();
            let restrict_weight = f32x4::maxf(
                f32x4::ZERO,
                f32x4::minf(
                    f32x4::ONE,
                    (dominant_edge_dot_horizontal_normal_squared / interpolation_scale
                        - interpolation_min)
                        * inverse_interpolation_span,
                ),
            );

            //Cylinder side vs triangle edge
            if use_side_egde_case.any() {
                //For the single contact case, all we need is to test the triangle edge versus the
                // plane formed by the cylinder side edge and the local normal.
                let cylinder_edge_to_dominant_edge_start_x = dominant_edge_start.x - closest_on_b.x;
                let cylinder_edge_to_dominant_edge_start_z = dominant_edge_start.z - closest_on_b.z;
                let numerator = cylinder_edge_to_dominant_edge_start_x * local_normal.z
                    - cylinder_edge_to_dominant_edge_start_z * local_normal.x;
                let edge_t = numerator / dominant_edge_dot_horizontal_normal;

                //The projection of the cylinder centerline onto the dominant edge.
                let inverse_edge_offset_length_squared = dominant_edge_length_squared.recip();
                let t_center = -Vec3x4::dot(
                    vec3x4(
                        cylinder_edge_to_dominant_edge_start_x,
                        dominant_edge_start.y,
                        cylinder_edge_to_dominant_edge_start_z,
                    ),
                    dominant_edge_offset,
                ) * inverse_edge_offset_length_squared;

                let projected_extent_offset = b.half_height
                    * f32x4::absf(dominant_edge_offset.y)
                    * inverse_edge_offset_length_squared;

                cylinder_t_min = t_center - projected_extent_offset;
                cylinder_t_max = t_center + projected_extent_offset;

                //Use one contact or two.
                let regular_contribution = restrict_weight
                    * f32x4::select(
                        t_center,
                        dominant_edge_dot_horizontal_normal_squared.lt(interpolation_min),
                        edge_t,
                    );

                let unrestrict_weight = f32x4::ONE - regular_contribution;
                cylinder_t_min = regular_contribution + unrestrict_weight * cylinder_t_min;
                cylinder_t_max = regular_contribution + unrestrict_weight * cylinder_t_max;

                cylinder_t_min = cylinder_t_min.clamp(f32x4::ZERO, f32x4::ONE);
                cylinder_t_max = cylinder_t_max.clamp(f32x4::ZERO, f32x4::ONE);

                //Calculate depth.
                let inverse_depth_denominator = -horizontal_normal_length_squared.recip();
                let depth_base = (cylinder_edge_to_dominant_edge_start_x * local_normal.x
                    + cylinder_edge_to_dominant_edge_start_z * local_normal.z)
                    * inverse_depth_denominator;
                let t_depth_scale = (dominant_edge_offset.x * local_normal.x
                    + dominant_edge_offset.z * local_normal.z)
                    * inverse_depth_denominator;

                depth_t_min = depth_base + t_depth_scale * cylinder_t_min;
                depth_t_max = depth_base + t_depth_scale * cylinder_t_max;

                let min_offset = dominant_edge_offset * cylinder_t_min;
                let max_offset = dominant_edge_offset * cylinder_t_max;

                //Temporarily, the computed offset_b data is stored in manifold.offset_a.
                manifold.offset_a[0] = Vec3x4::lane_select(
                    use_side_egde_case,
                    dominant_edge_start + min_offset,
                    manifold.offset_a[0],
                );
                manifold.offset_a[1] = Vec3x4::lane_select(
                    use_side_egde_case,
                    dominant_edge_start + max_offset,
                    manifold.offset_a[1],
                );
            }

            //Cylinder side vs triangle face
            let use_side_triangle_face = use_side & !use_triangle_edge_case;
            if use_side_triangle_face.any() {
                //Project cylinder side to triangle.
                let inverse_denominator = face_normal_a_dot_normal.recip();
                let xz_contribution = (local_triangle_center.x - closest_on_b.x)
                    * triangle_normal.x
                    + (local_triangle_center.z - closest_on_b.z) * triangle_normal.z;

                let t_min_to_triangle = (xz_contribution
                    + (local_triangle_center.y + b.half_height) * triangle_normal.y)
                    * inverse_denominator;
                let t_max_to_triangle = (xz_contribution
                    + (local_triangle_center.y - b.half_height) * triangle_normal.y)
                    * inverse_denominator;

                let min_on_triangle = vec3x4(
                    t_min_to_triangle * local_normal.x + closest_on_b.x,
                    t_min_to_triangle * local_normal.y - b.half_height,
                    t_min_to_triangle * local_normal.z + closest_on_b.z,
                );

                let max_on_triangle = vec3x4(
                    t_max_to_triangle * local_normal.x + closest_on_b.x,
                    t_max_to_triangle * local_normal.y + b.half_height,
                    t_max_to_triangle * local_normal.z + closest_on_b.z,
                );

                let min_to_max = max_on_triangle - min_on_triangle;

                // Check the intersection points between the three edges of the triangle and the
                // projected cylinder side.
                let numerator_ab = Vec3x4::dot(triangle_a - min_on_triangle, egde_plane_ab);
                let numerator_bc = Vec3x4::dot(triangle_b - min_on_triangle, egde_plane_bc);
                let numerator_ca = Vec3x4::dot(triangle_c - min_on_triangle, egde_plane_ca);

                let mut denominator_ab = Vec3x4::dot(min_to_max, egde_plane_ab);
                let mut denominator_bc = Vec3x4::dot(min_to_max, egde_plane_bc);
                let mut denominator_ca = Vec3x4::dot(min_to_max, egde_plane_ca);

                let threshold = f32x4::splat(1e-30f32);
                let negative_threshold = -threshold;

                let exiting_ab = denominator_ab.le(f32x4::ZERO);
                let exiting_bc = denominator_bc.le(f32x4::ZERO);
                let exiting_ca = denominator_ca.le(f32x4::ZERO);

                denominator_ab = f32x4::select(
                    f32x4::select(negative_threshold, exiting_ab, threshold),
                    f32x4::absf(denominator_ab).lt(threshold),
                    denominator_ab,
                );

                denominator_bc = f32x4::select(
                    f32x4::select(negative_threshold, exiting_bc, threshold),
                    f32x4::absf(denominator_bc).lt(threshold),
                    denominator_bc,
                );
                denominator_ca = f32x4::select(
                    f32x4::select(negative_threshold, exiting_ca, threshold),
                    f32x4::absf(denominator_ca).lt(threshold),
                    denominator_ca,
                );

                let edge_t_ab = numerator_ab / denominator_ab;
                let edge_t_bc = numerator_bc / denominator_bc;
                let edge_t_ca = numerator_ca / denominator_ca;

                let mut entry_ab = f32x4::select(f32x4::MIN, exiting_ab, edge_t_ab);
                let mut entry_bc = f32x4::select(f32x4::MIN, exiting_bc, edge_t_bc);
                let mut entry_ca = f32x4::select(f32x4::MIN, exiting_ca, edge_t_ca);
                let mut exit_ab = f32x4::select(edge_t_ab, exiting_ab, f32x4::MAX);
                let mut exit_bc = f32x4::select(edge_t_bc, exiting_bc, f32x4::MAX);
                let mut exit_ca = f32x4::select(edge_t_ca, exiting_ca, f32x4::MAX);

                let ca_is_dominant = !ab_is_dominant & !bc_is_dominant;

                entry_ab = f32x4::select(entry_ab * restrict_weight, ab_is_dominant, entry_ab);
                entry_bc = f32x4::select(entry_bc * restrict_weight, bc_is_dominant, entry_bc);
                entry_ca = f32x4::select(entry_ca * restrict_weight, ca_is_dominant, entry_ca);

                let unrestrict_weight = f32x4::ONE - restrict_weight;
                exit_ab = f32x4::select(
                    exit_ab * restrict_weight + unrestrict_weight,
                    ab_is_dominant,
                    exit_ab,
                );
                exit_bc = f32x4::select(
                    exit_bc * restrict_weight + unrestrict_weight,
                    bc_is_dominant,
                    exit_bc,
                );
                exit_ca = f32x4::select(
                    exit_ca * restrict_weight + unrestrict_weight,
                    ca_is_dominant,
                    exit_ca,
                );

                //Find intersection t.
                let side_triangle_cylinder_tmin =
                    f32x4::maxf(entry_ab, f32x4::maxf(entry_bc, entry_ca));
                let side_triangle_cylinder_tmax =
                    f32x4::minf(exit_ab, f32x4::minf(exit_bc, exit_ca));

                let use_vertex_fallback = side_triangle_cylinder_tmax
                    .lt(side_triangle_cylinder_tmin)
                    & use_side_triangle_face;
                let ab_contributed_bound = edge_t_ab.eq(side_triangle_cylinder_tmin)
                    | edge_t_ab.eq(side_triangle_cylinder_tmax);
                let bc_contributed_bound = edge_t_bc.eq(side_triangle_cylinder_tmin)
                    | edge_t_bc.eq(side_triangle_cylinder_tmax);
                let ca_contributed_bound = edge_t_ca.eq(side_triangle_cylinder_tmin)
                    | edge_t_ca.eq(side_triangle_cylinder_tmax);
                let use_a = ca_contributed_bound & ab_contributed_bound;
                let use_b = ab_contributed_bound & bc_contributed_bound;

                let vertex_fallback = Vec3x4::lane_select(
                    use_a,
                    triangle_a,
                    Vec3x4::lane_select(use_b, triangle_b, triangle_c),
                );

                cylinder_t_min = f32x4::select(
                    side_triangle_cylinder_tmin.clamp(f32x4::ZERO, f32x4::ONE),
                    use_side_triangle_face,
                    cylinder_t_min,
                );

                cylinder_t_max = f32x4::select(
                    side_triangle_cylinder_tmax.clamp(f32x4::ZERO, f32x4::ONE),
                    use_side_triangle_face,
                    cylinder_t_max,
                );

                //Temporarily, the computed offset_b data is stored in manifold.offset_a.
                manifold.offset_a[0] = Vec3x4::lane_select(
                    use_side_triangle_face,
                    min_on_triangle + min_to_max * cylinder_t_min,
                    manifold.offset_a[0],
                );

                manifold.offset_a[1] = Vec3x4::lane_select(
                    use_side_triangle_face,
                    min_on_triangle + min_to_max * cylinder_t_max,
                    manifold.offset_a[1],
                );
                manifold.offset_a[0] =
                    Vec3x4::lane_select(use_vertex_fallback, vertex_fallback, manifold.offset_a[0]);

                let inverse_depth_denominator =
                    (local_normal.x * local_normal.x + local_normal.z * local_normal.z).recip();

                depth_t_min = f32x4::select(
                    (local_normal.x * (closest_on_b.x - manifold.offset_a[0].x)
                        + local_normal.z * (closest_on_b.z - manifold.offset_a[0].z))
                        * inverse_depth_denominator,
                    use_side_triangle_face,
                    depth_t_min,
                );
                depth_t_max = f32x4::select(
                    (local_normal.x * (closest_on_b.x - manifold.offset_a[1].x)
                        + local_normal.z * (closest_on_b.z - manifold.offset_a[1].z))
                        * inverse_depth_denominator,
                    use_side_triangle_face,
                    depth_t_max,
                );
            }

            manifold.feature_id[0] = u32x4::select(u32x4::ZERO, use_side, manifold.feature_id[0]);
            manifold.feature_id[1] = u32x4::select(u32x4::ONE, use_side, manifold.feature_id[1]);
            manifold.depth[0] = f32x4::select(depth_t_min, use_side, manifold.depth[0]);
            manifold.depth[1] = f32x4::select(depth_t_max, use_side, manifold.depth[1]);

            manifold.contact_exists[0] = bool32x4::select(
                depth_t_min.gt(depth_threshold),
                use_side,
                manifold.contact_exists[0],
            );
            manifold.contact_exists[1] = bool32x4::select(
                depth_t_max.gt(depth_threshold) & cylinder_t_max.gt(cylinder_t_min),
                use_side,
                manifold.contact_exists[1],
            );

            manifold.contact_exists[2] =
                bool32x4::select(bool32x4::FALSE, use_side, manifold.contact_exists[2]);
            manifold.contact_exists[3] =
                bool32x4::select(bool32x4::FALSE, use_side, manifold.contact_exists[3]);
        }

        manifold.normal = (world_rotation_b * local_normal).as_unit_vec3x4_unchecked();

        manifold.offset_a[0] = world_rotation_b * (manifold.offset_a[0] + local_offset_b);
        manifold.offset_a[1] = world_rotation_b * (manifold.offset_a[1] + local_offset_b);
        manifold.offset_a[2] = world_rotation_b * (manifold.offset_a[2] + local_offset_b);
        manifold.offset_a[3] = world_rotation_b * (manifold.offset_a[3] + local_offset_b);
    }
}

struct EdgeInterectionInfo {
    edge_start: Vec2x4,
    edge_offset: Vec2x4,
    t_min: f32x4,
    t_max: f32x4,
}

struct TangentInfo {
    tangent_a: Vec2x4,
    tangent_ab: Vec2x4,
    tangent_b: Vec2x4,
    tangent_bc: Vec2x4,
    tangent_c: Vec2x4,
    tangent_ca: Vec2x4,
}

fn try_add_edge(
    intersection_info: &EdgeInterectionInfo,
    allow_contacts: bool32x4,
    edge_id: u32x4,
    pair_count: usize,
    candidates: &mut Candidates<10>,
) {
    let mut candidate = ManifoldCandidateWide {
        x: intersection_info.edge_start.x
            + intersection_info.edge_offset.x * intersection_info.t_min,
        y: intersection_info.edge_start.y
            + intersection_info.edge_offset.y * intersection_info.t_min,
        depth: f32x4::ZERO,
        feature_id: edge_id,
    };

    candidates.add(
        &candidate,
        allow_contacts
            & intersection_info.t_min.lt(intersection_info.t_max)
            & intersection_info.t_min.gt(f32x4::ZERO),
        pair_count,
    );

    candidate.feature_id = edge_id + u32x4::const_splat(4);
    candidate.x =
        intersection_info.edge_start.x + intersection_info.edge_offset.x * intersection_info.t_max;
    candidate.y =
        intersection_info.edge_start.y + intersection_info.edge_offset.y * intersection_info.t_max;
    candidates.add(
        &candidate,
        allow_contacts & intersection_info.t_max.gt(f32x4::ZERO),
        pair_count,
    );
}

fn try_add_interior_point(
    point: Vec2x4,
    feature_id: u32x4,
    tangent_info: &TangentInfo,
    allow_contact: bool32x4,
    pair_count: usize,
    candidates: &mut Candidates<10>,
) {
    let ap = point - tangent_info.tangent_a;
    let bp = point - tangent_info.tangent_b;
    let cp = point - tangent_info.tangent_c;

    let ab_dot = ap.x * tangent_info.tangent_ab.y - ap.y * tangent_info.tangent_ab.x;
    let bc_dot = bp.x * tangent_info.tangent_bc.y - bp.y * tangent_info.tangent_bc.x;
    let ca_dot = cp.x * tangent_info.tangent_ca.y - cp.y * tangent_info.tangent_ca.x;

    let case0 = ab_dot.gt(f32x4::ZERO) & bc_dot.gt(f32x4::ZERO) & ca_dot.gt(f32x4::ZERO);
    let case1 = ab_dot.lt(f32x4::ZERO) & bc_dot.lt(f32x4::ZERO) & ca_dot.lt(f32x4::ZERO);
    let contained = allow_contact & (case0 | case1);

    let candidate = ManifoldCandidateWide {
        x: point.x,
        y: point.y,
        depth: f32x4::ZERO,
        feature_id,
    };

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