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::*;
use glam_det::{Dot, Mat3x4, UnitQuat, UnitQuatx4, UnitVec3x4, Vec2x4, Vec3, Vec3x4};

use super::capsule_cylinder_tester::get_contact_interval_between_segments;
use super::common::{NormalizeExt, EPS_10, EPS_12, EPS_6, FRAC_1_SQRT_2};
use super::tootbird;
use crate::collision_tasks::common::EPS_14;
use crate::collision_tasks::traits::TransformWide;
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::{Cylinder, CylinderWide};
use crate::traits::{
    ContactContext, ContactManifoldWide, CreateShapeWide, OrientationWide, PairWideTest,
};
use crate::{ConvexContactManifold, PairTest, ShapeContainer, ShapeTester};

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

    // 4 manifold in Convex4ContactManifoldWide
    fn test(
        a: &CylinderWide,
        b: &CylinderWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        // There are 3 kinds of intersection for cylinder vs cylinder:
        // 1. Cap vs cap
        //   * Any cylinder axis is parallel with contact normal. (4 contacts)
        //   * None of the cylinder axis is parallel with contact normal. (1 contact)
        // 2. Cap vs side (2 contacts)
        // 3. Side vs side (2 contacts)

        // Note: The following compute would be mostly in b local space
        let pair_count_i32 =
            i32::try_from(contact_context.pair_count).expect("pair_count must in range");

        let rb = Mat3x4::from_quat(*contact_context.orientation_b);
        let rb_inverse = rb.transpose();
        let ra = rb_inverse * Mat3x4::from_quat(*contact_context.orientation_a);
        let local_offset_b = rb_inverse * contact_context.offset_b;
        let local_offset_a = -local_offset_b;
        let normal_a = local_offset_a.normalize_or(Vec3x4::Y, EPS_10);
        let inactive_lanes = i32x4::splat(pair_count_i32).le(i32x4::from([0, 1, 2, 3]));
        let minimum_accepted_depth = -contact_context.speculative_margin;
        let epsilon_scale = a.half_height.max(a.radius).min(b.half_height.max(b.radius)) * EPS_6;

        let toot_bird_result = tootbird::find_minimum_depth(
            b,
            a,
            &TransformWide::new(local_offset_a, &ra),
            normal_a,
            &tootbird::IterContext::new(
                inactive_lanes,
                epsilon_scale,
                minimum_accepted_depth,
                25,
                None,
                false,
            ),
        );
        let active_lanes = !(inactive_lanes | toot_bird_result.depth.le(minimum_accepted_depth));

        if active_lanes.none() {
            return;
        }

        // Prepare for the intersecting kinds. Generate by diffetent kinds of cap intersected
        // combination.
        let contact_normal_dot_a_y = ra.y_axis.dot(toot_bird_result.normal);
        let inverse_contact_normal_dot_a_y = contact_normal_dot_a_y.recip();
        let inverse_contact_normal_y = toot_bird_result.normal.y.recip();
        let cap_a_intersected = contact_normal_dot_a_y.absf().gt(FRAC_1_SQRT_2);
        let cap_b_intersected = toot_bird_result.normal.y.absf().gt(FRAC_1_SQRT_2);
        let cap_vs_cap = cap_a_intersected & cap_b_intersected & active_lanes;
        let cap_vs_side = (cap_a_intersected ^ cap_b_intersected) & active_lanes;
        let side_vs_side = (!(cap_a_intersected | cap_b_intersected)) & active_lanes;

        // Prepare cap data.
        let is_neg_cap_a = contact_normal_dot_a_y.gt(f32x4::ZERO);
        let cap_normal_a = Vec3x4::lane_select(is_neg_cap_a, -ra.y_axis, ra.y_axis);
        let cap_center_a =
            ra.y_axis * ((-a.half_height).select(is_neg_cap_a, a.half_height)) + local_offset_a;
        let cap_center_b_y =
            (-b.half_height).select(toot_bird_result.normal.y.lt(f32x4::ZERO), b.half_height);

        let contact_on_b = Vec2x4::new(
            toot_bird_result.closest_point_on_a.x,
            toot_bird_result.closest_point_on_a.z,
        );
        let mut contacts: [Vec3x4; 4] = Default::default();

        if cap_vs_cap.any() {
            const PARALLEL_THRESHOLD: f32x4 = f32x4::const_splat(0.9999);
            const INVERSE_PARALLEL_INTERPOLATION_SPAN: f32x4 = f32x4::const_splat(1.0 / 0.00005);
            let abs_contact_normal_dot_a_y = contact_normal_dot_a_y.absf();
            let abs_contact_normal_dot_b_y = toot_bird_result.normal.y.absf();
            // Cap normal parallel with contact_normal
            let has_cap_normal_parallel = abs_contact_normal_dot_a_y.gt(PARALLEL_THRESHOLD)
                | abs_contact_normal_dot_b_y.gt(PARALLEL_THRESHOLD);

            let mut cap_contacts: [Vec2x4; 4] = Default::default();
            // If no parallel, we'll use the deepest point on b as only one contact.
            cap_contacts[0] = contact_on_b;

            if (has_cap_normal_parallel & active_lanes).any() {
                // Project the cap center of A onto the cap of B by contact normal.
                // Create line_0 on B cap with projected point and B cap center.
                // cap_contact_0~1 is the intersection of line_0 and B cap, and cap_contact_2~3 is
                // the intersection of line_1 and B cap.
                let cap_center_a_on_b = project_onto_cap_b(
                    cap_center_b_y,
                    inverse_contact_normal_y,
                    toot_bird_result.normal,
                    cap_center_a,
                );
                let (length_cap_center_a_on_b_xz, normal_cap_center_a_on_b_xz) =
                    cap_center_a_on_b.normalize_and_length_or(Vec2x4::X, EPS_14);
                let line_0_start_on_b = normal_cap_center_a_on_b_xz * b.radius;
                let line_0_end_on_b = -line_0_start_on_b;
                let line_0_direction_on_b = line_0_end_on_b - line_0_start_on_b;

                // Project line_0 to cap a, and test it with circle.
                let line_0_start_on_a = project_onto_cap_a(
                    cap_center_b_y,
                    cap_center_a,
                    &ra,
                    inverse_contact_normal_dot_a_y,
                    toot_bird_result.normal,
                    line_0_start_on_b,
                );
                let line_0_end_on_a = project_onto_cap_a(
                    cap_center_b_y,
                    cap_center_a,
                    &ra,
                    inverse_contact_normal_dot_a_y,
                    toot_bird_result.normal,
                    line_0_end_on_b,
                );
                let line_0_direction_on_a = line_0_end_on_a - line_0_start_on_a;
                let (contact_0_t_min_a, contact_0_t_max_a) =
                    intersect_line_circle(line_0_start_on_a, line_0_direction_on_a, a.radius);
                let line_0_t_min = contact_0_t_min_a.max(f32x4::ZERO);
                let line_0_t_max = contact_0_t_max_a.min(f32x4::ONE);
                cap_contacts[0] = line_0_direction_on_b * line_0_t_min + line_0_start_on_b;
                cap_contacts[1] = line_0_direction_on_b * line_0_t_max + line_0_start_on_b;

                let circle_intersection_t = (length_cap_center_a_on_b_xz
                    + (b.radius * b.radius - a.radius * a.radius)
                        * length_cap_center_a_on_b_xz.recip())
                    * f32x4::HALF;
                let line_1_start_t = circle_intersection_t
                    .max(f32x4::ZERO)
                    .min(length_cap_center_a_on_b_xz);
                let line_1_start_on_b = normal_cap_center_a_on_b_xz * line_1_start_t;
                let line_1_direction_on_b = Vec2x4::new(
                    normal_cap_center_a_on_b_xz.y,
                    -normal_cap_center_a_on_b_xz.x,
                );

                let line_1_end_on_b = line_1_start_on_b + line_1_direction_on_b;
                let line_1_start_on_a = project_onto_cap_a(
                    cap_center_b_y,
                    cap_center_a,
                    &ra,
                    inverse_contact_normal_dot_a_y,
                    toot_bird_result.normal,
                    line_1_start_on_b,
                );
                let line_1_end_on_a = project_onto_cap_a(
                    cap_center_b_y,
                    cap_center_a,
                    &ra,
                    inverse_contact_normal_dot_a_y,
                    toot_bird_result.normal,
                    line_1_end_on_b,
                );
                let line_1_direction_on_a = line_1_end_on_a - line_1_start_on_a;

                let (line_1_t_min_a, line_1_t_max_a) =
                    intersect_line_circle(line_1_start_on_a, line_1_direction_on_a, a.radius);
                let (line_1_t_min_b, line_1_t_max_b) =
                    intersect_line_circle(line_1_start_on_b, line_1_direction_on_b, b.radius);

                let line_1_t_min = line_1_t_min_a.max(line_1_t_min_b);
                let line_1_t_max = line_1_t_max_a.min(line_1_t_max_b);

                cap_contacts[2] = line_1_direction_on_b * line_1_t_min + line_1_start_on_b;
                cap_contacts[3] = line_1_direction_on_b * line_1_t_max + line_1_start_on_b;

                // There are 4 cap_contacts for parallel, and we consider parallel as
                // abs_contact_normal_dot_a_y > PARALLEL_THRESHOLD(below as unparallel).
                // We will lerp between threshold and absolutely parallel(1), since
                // unparallel has only one contact: contact_on_b.
                // Select one of the contact point, more close to threshold, move it more close to
                // contact_on_b.
                let weight_a_parallel = ((abs_contact_normal_dot_a_y - PARALLEL_THRESHOLD)
                    * INVERSE_PARALLEL_INTERPOLATION_SPAN)
                    .clamp(f32x4::ZERO, f32x4::ONE);
                let weight_b_parallel = ((abs_contact_normal_dot_b_y - PARALLEL_THRESHOLD)
                    * INVERSE_PARALLEL_INTERPOLATION_SPAN)
                    .clamp(f32x4::ZERO, f32x4::ONE);
                let parallel_weight = weight_a_parallel * weight_b_parallel;
                let manifold_center_to_contact_on_b = contact_on_b - line_1_start_on_b;
                let replace_dot_0 = normal_cap_center_a_on_b_xz.x
                    * manifold_center_to_contact_on_b.x
                    + normal_cap_center_a_on_b_xz.y * manifold_center_to_contact_on_b.y;
                let replace_dot_2 = line_1_direction_on_b.x * manifold_center_to_contact_on_b.x
                    + line_1_direction_on_b.y * manifold_center_to_contact_on_b.y;
                let replace_0_or_1 = replace_dot_0.absf().gt(replace_dot_2.absf());

                let mut replace = [bool32x4::FALSE; 4];
                replace[0] =
                    !has_cap_normal_parallel | (replace_dot_0.gt(f32x4::ZERO) & replace_0_or_1);
                replace[1] = replace_dot_0.le(f32x4::ZERO) & replace_0_or_1;
                replace[2] = replace_dot_2.lt(f32x4::ZERO) & !replace_0_or_1;
                replace[3] = replace_dot_2.ge(f32x4::ZERO) & !replace_0_or_1;

                // parallel_weight range: `[0, 1]`, `0` = contact_on_b, `1` = cap_contact.
                for (i, cap_contact) in cap_contacts.iter_mut().enumerate() {
                    *cap_contact = Vec2x4::lane_select(
                        replace[i],
                        contact_on_b.lerp(*cap_contact, parallel_weight),
                        *cap_contact,
                    );
                    if i > 0 {
                        contacts[i] = from_cap_b_to_3d(*cap_contact, cap_center_b_y);
                    }
                }

                manifold.contact_exists[1] =
                    cap_vs_cap & line_0_t_max.gt(line_0_t_min) & has_cap_normal_parallel;
                manifold.contact_exists[2] = manifold.contact_exists[1];
                manifold.contact_exists[3] =
                    manifold.contact_exists[1] & line_1_t_max.gt(line_1_t_min);
            }

            contacts[0] = from_cap_b_to_3d(cap_contacts[0], cap_center_b_y);
            manifold.contact_exists[0] = cap_vs_cap;
        }

        // Prepare side data.
        // The side is generate from toot_bird_result's contacts.
        let ax = ra.x_axis.dot(toot_bird_result.normal);
        let az = ra.z_axis.dot(toot_bird_result.normal);
        let inverse_xz_normal_length_a = (ax * ax + az * az).sqrtf().recip();
        let x_scale = ax * inverse_xz_normal_length_a;
        let z_scale = az * inverse_xz_normal_length_a;
        let side_selected_normal_a_x = ra.x_axis * x_scale;
        let side_selected_normal_a_z = ra.z_axis * z_scale;
        let side_selected_normal_a = side_selected_normal_a_x + side_selected_normal_a_z;
        let contact_on_a =
            toot_bird_result.closest_point_on_a - toot_bird_result.normal * toot_bird_result.depth;
        let a_to_contact_on_a_xz_offset = (contact_on_a - local_offset_a)
            .reject_from_normalized(ra.y_axis.as_unit_vec3x4_unchecked());
        let side_center_a = a_to_contact_on_a_xz_offset + local_offset_a;
        let side_center_b = Vec3x4::new(contact_on_b.x, f32x4::ZERO, contact_on_b.y);

        // Cap normalor or side normal on A. (A cap vs B side) or (A side vs B cap/side)
        let mut selected_normal_a = cap_normal_a;
        // Cap center or side center on A. As same logic as selected_normal_a.
        let mut selected_center_a = cap_center_a;

        if cap_vs_side.any() {
            // Project side line to cap, and test it with circle.
            let side_line_end_a = side_center_a + ra.y_axis;
            let side_line_end_b = Vec3x4::new(side_center_b.x, f32x4::ONE, side_center_b.z);
            let projected_line_start_b_on_a = project_onto_cap_a_point3d(
                cap_center_a,
                &ra,
                inverse_contact_normal_dot_a_y,
                toot_bird_result.normal,
                side_center_b,
            );
            let projected_line_start_a_on_b = project_onto_cap_b(
                cap_center_b_y,
                inverse_contact_normal_y,
                toot_bird_result.normal,
                side_center_a,
            );
            let projected_line_end_b_on_a = project_onto_cap_a_point3d(
                cap_center_a,
                &ra,
                inverse_contact_normal_dot_a_y,
                toot_bird_result.normal,
                side_line_end_b,
            );
            let projected_line_end_a_on_b = project_onto_cap_b(
                cap_center_b_y,
                inverse_contact_normal_y,
                toot_bird_result.normal,
                side_line_end_a,
            );

            // Select projected edge line.
            let projected_line_start = Vec2x4::lane_select(
                cap_a_intersected,
                projected_line_start_b_on_a,
                projected_line_start_a_on_b,
            );
            let projected_line_end = Vec2x4::lane_select(
                cap_a_intersected,
                projected_line_end_b_on_a,
                projected_line_end_a_on_b,
            );
            // Select cap circle.
            let radius = a.radius.select(cap_a_intersected, b.radius);
            let side_half_length = b.half_height.select(cap_a_intersected, a.half_height);
            let projected_line_direction = projected_line_end - projected_line_start;
            let (mut t_min, mut t_max) =
                intersect_line_circle(projected_line_start, projected_line_direction, radius);
            t_min = t_min.clamp(-side_half_length, side_half_length);
            t_max = t_max.min(side_half_length);

            let contact_0_for_cap_a = Vec3x4::new(side_center_b.x, t_min, side_center_b.z);
            let contact_1_for_cap_a = Vec3x4::new(side_center_b.x, t_max, side_center_b.z);
            let contact_0_for_cap_b = Vec3x4::new(
                projected_line_start.x + t_min * projected_line_direction.x,
                cap_center_b_y,
                projected_line_start.y + t_min * projected_line_direction.y,
            );
            let contact_1_for_cap_b = Vec3x4::new(
                projected_line_start.x + t_max * projected_line_direction.x,
                cap_center_b_y,
                projected_line_start.y + t_max * projected_line_direction.y,
            );
            let cap_side_contact_0 =
                Vec3x4::lane_select(cap_a_intersected, contact_0_for_cap_a, contact_0_for_cap_b);
            let cap_side_contact_1 =
                Vec3x4::lane_select(cap_a_intersected, contact_1_for_cap_a, contact_1_for_cap_b);
            contacts[0] = Vec3x4::lane_select(cap_vs_side, cap_side_contact_0, contacts[0]);
            contacts[1] = Vec3x4::lane_select(cap_vs_side, cap_side_contact_1, contacts[1]);
            manifold.contact_exists[0] = manifold.contact_exists[0] | cap_vs_side;
            manifold.contact_exists[1] =
                (t_max.gt(t_min) & cap_vs_side) | manifold.contact_exists[1];

            let cap_side_selected_normal_a =
                Vec3x4::lane_select(cap_a_intersected, cap_normal_a, side_selected_normal_a);
            selected_normal_a =
                Vec3x4::lane_select(cap_vs_side, cap_side_selected_normal_a, selected_normal_a);

            let cap_side_selected_position_a =
                Vec3x4::lane_select(cap_a_intersected, cap_center_a, side_center_a);
            selected_center_a =
                Vec3x4::lane_select(cap_vs_side, cap_side_selected_position_a, selected_center_a);
        }

        if side_vs_side.any() {
            // Just like segment vs segment we did in capsule vs cylinder.

            let side_center_a_to_b = -side_center_a;
            let xz_normal_length_squared_b = toot_bird_result.normal.x * toot_bird_result.normal.x
                + toot_bird_result.normal.z * toot_bird_result.normal.z;
            let inverse_xz_normal_length_squared_b = xz_normal_length_squared_b.recip();

            let (contact_t_min, contact_t_max) = get_contact_interval_between_segments(
                a.half_height,
                b.half_height,
                &ra.y_axis,
                &toot_bird_result.normal.as_vec3x4(),
                inverse_xz_normal_length_squared_b,
                &side_center_a_to_b,
            );

            contacts[0].x = contact_on_b.x.select(side_vs_side, contacts[0].x);
            contacts[0].y = contact_t_min.select(side_vs_side, contacts[0].y);
            contacts[0].z = contact_on_b.y.select(side_vs_side, contacts[0].z);
            contacts[1].x = contact_on_b.x.select(side_vs_side, contacts[1].x);
            contacts[1].y = contact_t_max.select(side_vs_side, contacts[1].y);
            contacts[1].z = contact_on_b.y.select(side_vs_side, contacts[1].z);
            manifold.contact_exists[0] = side_vs_side | manifold.contact_exists[0];
            manifold.contact_exists[1] =
                (contact_t_max.gt(contact_t_min) & side_vs_side) | manifold.contact_exists[1];
            selected_normal_a =
                Vec3x4::lane_select(side_vs_side, side_selected_normal_a, selected_normal_a);
            selected_center_a = Vec3x4::lane_select(side_vs_side, side_center_a, selected_center_a);
        }
        // For side vs side, recip is safe, since it's less than 45 degree bewteen contact normal
        // and cylinder axis.
        let inverse_selected_normal_a_dot_contact_normal =
            selected_normal_a.dot(toot_bird_result.normal).recip();

        manifold.normal = contact_context.orientation_b * toot_bird_result.normal;

        let context = TransformContactContext {
            local_selected_position_a: &selected_center_a,
            local_selected_normal_a: &selected_normal_a,
            local_offset_b: &local_offset_b,
        };
        for (i, contact) in contacts.into_iter().enumerate() {
            (manifold.offset_a[i], manifold.depth[i]) = transform_contact(
                contact,
                &context,
                inverse_selected_normal_a_dot_contact_normal,
                minimum_accepted_depth,
                &rb,
                &mut manifold.contact_exists[i],
            );
            manifold.feature_id[i] = u32x4::const_splat(i as u32);
            manifold.offset_a[i] += -manifold.normal * manifold.depth[i];
        }
    }
}

struct TransformContactContext<'a> {
    local_selected_position_a: &'a Vec3x4,
    local_selected_normal_a: &'a Vec3x4,
    local_offset_b: &'a Vec3x4,
}
#[inline(always)]
fn transform_contact(
    contact: Vec3x4,
    context: &TransformContactContext,
    inverse_selected_normal_a_dot_contact_normal: f32x4,
    minimum_accepted_depth: f32x4,
    orientation_b: &impl OrientationWide,
    contact_exists: &mut bool32x4,
) -> (Vec3x4, f32x4) {
    let selected_offset = contact - context.local_selected_position_a;
    let t_distance = selected_offset.dot(context.local_selected_normal_a);
    let depth = t_distance * inverse_selected_normal_a_dot_contact_normal;
    let local_a_to_contact = contact + context.local_offset_b;
    let a_to_contact = orientation_b.mul_vec3(local_a_to_contact);
    *contact_exists = *contact_exists & (depth.ge(minimum_accepted_depth));
    (a_to_contact, depth)
}

#[inline(always)]
fn project_onto_cap_a(
    cap_center_b_y: f32x4,
    cap_center_a: Vec3x4,
    ra: &Mat3x4,
    inverse_contact_normal_dot_a_y: f32x4,
    conract_normal: UnitVec3x4,
    point: Vec2x4,
) -> Vec2x4 {
    let point3d = Vec3x4::new(point.x, cap_center_b_y, point.y);
    project_onto_cap_a_point3d(
        cap_center_a,
        ra,
        inverse_contact_normal_dot_a_y,
        conract_normal,
        point3d,
    )
}

#[inline(always)]
fn project_onto_cap_a_point3d(
    cap_center_a: Vec3x4,
    ra: &Mat3x4,
    inverse_contact_normal_dot_a_y: f32x4,
    conract_normal: UnitVec3x4,
    point: Vec3x4,
) -> Vec2x4 {
    let point_to_cap_center_a = cap_center_a - point;
    let t_distance = point_to_cap_center_a.dot(ra.y_axis);
    let t_b_on_a = t_distance * inverse_contact_normal_dot_a_y;
    let projection_offset_b = conract_normal * t_b_on_a;
    let projected_point = point + projection_offset_b;
    let cap_center_a_to_projected_point = projected_point - cap_center_a;
    Vec2x4::new(
        cap_center_a_to_projected_point.dot(ra.x_axis),
        cap_center_a_to_projected_point.dot(ra.z_axis),
    )
}

#[inline(always)]
fn intersect_line_circle(
    line_position: Vec2x4,
    line_direction: Vec2x4,
    radius: f32x4,
) -> (f32x4, f32x4) {
    let a = line_direction.length_squared();
    let inverse_a = a.recip();
    let b = line_position.dot(line_direction);
    let mut c = line_position.dot(line_position);
    let radius_squared = radius * radius;
    c -= radius_squared;
    let t_offset = f32x4::sqrtf(f32x4::max(f32x4::ZERO, b * b - a * c)) * inverse_a;
    let t_base = -b * inverse_a;

    let use_fallback = a.lt(EPS_12);
    let t_min = f32x4::ZERO.select(use_fallback, t_base - t_offset);
    let t_max = f32x4::ZERO.select(use_fallback, t_base + t_offset);

    (t_min, t_max)
}

#[inline(always)]
#[must_use]
pub fn project_onto_cap_b(
    cap_center_b_y: f32x4,
    inverse_contact_normal_y: f32x4,
    contact_normal: UnitVec3x4,
    point: Vec3x4,
) -> Vec2x4 {
    let t_a_on_b = (point.y - cap_center_b_y) * inverse_contact_normal_y;
    Vec2x4::new(
        point.x - contact_normal.x * t_a_on_b,
        point.z - contact_normal.z * t_a_on_b,
    )
}

#[inline(always)]
fn from_cap_b_to_3d(contact: Vec2x4, cap_center_b_y: f32x4) -> Vec3x4 {
    Vec3x4::new(contact.x, cap_center_b_y, contact.y)
}

impl_pair_narrowphase!(Cylinder, Cylinder, CylinderWide, CylinderWide, 4);