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::{
    bool32x4, f32x4, i32x4, u32x4, Bool, Float, FloatConstEx, Num, NumConstEx, PartialOrdEx, Signed,
};
use glam_det::{Cross, Dot, UnitQuat, UnitQuatx4, Vec2x4, Vec3, Vec3x4};

use crate::collision_tasks::common::{
    NormalizeExt, EPS_12, EPS_15, EPS_5, EPS_6, EPS_7, FRAC_1_SQRT_2,
};
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::{Capsule, CapsuleWide, Cylinder, CylinderWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairWideTest};
use crate::{ConvexContactManifold, PairTest, ShapeContainer, ShapeTester};

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

    // 2 manifold in Convex4ContactManifoldWide
    fn test(
        a: &CapsuleWide,
        b: &CylinderWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        // * 1. Test capsule line segment and cylinder, we can get normal and depth from closest
        //   point pair.
        // * 2. Get contacts on cylinder side.
        // * 3. Get contacts on cylinder cap.
        // * 4. Get contacts and push into world space.
        let pair_count_i32 =
            i32::try_from(contact_context.pair_count).expect("pair_count must in range");
        // Note: The following compute would be mostly in b local space
        let inverse_orientation_b = contact_context.orientation_b.inverse();
        let rotation_a = inverse_orientation_b * contact_context.orientation_a;
        let direction_a = rotation_a * Vec3x4::Y;
        let offset_a_to_b = inverse_orientation_b * contact_context.offset_b; // b-a in b local
        let local_offset_a = -offset_a_to_b;
        let mut inactive_lanes = i32x4::splat(pair_count_i32).le(i32x4::from([0, 1, 2, 3]));

        // 1. Get normal and depth by capsule line segment vs cylinder.
        //    * If the segment intersects the cylinder, mark as deeply_intersected, then calibrate
        //      normal for it.
        //    * If the distance > capsule.radius + margin, mark as inactive.
        let closest_offset = get_closest_offset_between_line_segment_and_cylinder(
            &local_offset_a,
            &direction_a,
            a.half_height,
            b,
            inactive_lanes,
        );
        let distance_from_cylinder_to_line_segment = closest_offset.length();
        // Mark intersecting deeply if capsule line_segment intersect cylinder.
        let deeply_intersected = distance_from_cylinder_to_line_segment.lt(EPS_6);

        // Normal of closet points from cylinder to capsule line segment
        // If distance is zero(deeply_intersected), the normal is INFINITY, calibrate it later.
        let mut local_normal = closest_offset / distance_from_cylinder_to_line_segment;
        // The depth between line segment and cylinder is negative(not
        // intersect) or MAX(intersect and calibrate later).
        let mut depth =
            f32x4::MAX.select(deeply_intersected, -distance_from_cylinder_to_line_segment);

        let negative_margin = -contact_context.speculative_margin;
        // Set inactive if line segment to cylinder is greater than a.radius + margin
        inactive_lanes = (depth + a.radius).lt(negative_margin) | inactive_lanes;

        // If deeply_intersected, local_normal is INFINITY, depth is MAX, so calibrate it now.
        if (deeply_intersected & (!inactive_lanes)).any() {
            let calibrate_info = CalibrateInfo::new(&direction_a, &local_offset_a, b);
            // 1.1 Calibrate normal and depth for deeply_intersected.
            calibrate_normal_and_depth(
                &mut depth,
                &mut local_normal,
                &calibrate_info,
                a.half_height,
                &offset_a_to_b,
                deeply_intersected,
            );
        }

        // We add consideration of the capsule's radiu now.
        depth += a.radius;
        inactive_lanes = depth.lt(negative_margin) | inactive_lanes;
        if inactive_lanes.all() {
            // No contact
            manifold.reset(2);
            return;
        }

        // 2. Get contacts on cylinder side by normal and depth.
        let inverse_horizontal_normal_length_squared =
            (local_normal.x * local_normal.x + local_normal.z * local_normal.z).recip();
        let scale = b.radius * inverse_horizontal_normal_length_squared.sqrtf();
        let cylinder_segment_offset_x = local_normal.x * scale;
        let cylinder_segment_offset_z = local_normal.z * scale;
        let a_to_side_segment_center = Vec3x4::new(
            offset_a_to_b.x + cylinder_segment_offset_x,
            offset_a_to_b.y,
            offset_a_to_b.z + cylinder_segment_offset_z,
        );
        let (contact_t_min, contact_t_max) = get_contact_interval_between_segments(
            a.half_height,
            b.half_height,
            &direction_a,
            &local_normal,
            inverse_horizontal_normal_length_squared,
            &a_to_side_segment_center,
        );

        let mut contact_0 = Vec3x4::new(
            cylinder_segment_offset_x,
            contact_t_min,
            cylinder_segment_offset_z,
        );
        let mut contact_1 = Vec3x4::new(
            cylinder_segment_offset_x,
            contact_t_max,
            cylinder_segment_offset_z,
        );
        let mut contact_count = i32x4::ONE.select(
            (contact_t_min - contact_t_max)
                .absf()
                .lt(b.half_height * EPS_5),
            i32x4::TWO,
        );

        // 3. Get contacts on cylinder cap.
        // If the normal y is greater than 45°, the cylinder cap is investigated.
        let cap_intersected = local_normal.y.absf().gt(FRAC_1_SQRT_2) & (!inactive_lanes);

        if cap_intersected.any() {
            let calibrate_info = CalibrateInfo::new(&direction_a, &local_offset_a, b);
            calibrate_cap_contacts(
                &mut contact_0,
                &mut contact_1,
                &mut contact_count,
                &calibrate_info,
                &local_normal,
                a.half_height,
                cap_intersected,
            );
        }

        // 4. Push the contacts into world space.

        // Convert depth into world space
        let depth_normal = local_normal.cross(direction_a).cross(direction_a);
        let local_normal_dot_depth_normal = depth_normal.dot(local_normal);
        let inverse_local_normal_dot_depth_normal = local_normal_dot_depth_normal.recip();
        let offset_0 = offset_a_to_b + contact_0;
        let offset_1 = offset_a_to_b + contact_1;

        // t = offset.dot(depth_normal) / (depth_normal.dot(local_normal))
        let t_0 = offset_0.dot(depth_normal) * inverse_local_normal_dot_depth_normal;
        let t_1 = offset_1.dot(depth_normal) * inverse_local_normal_dot_depth_normal;

        manifold.depth[0] = a.radius + t_0;
        manifold.depth[1] = a.radius + t_1;
        // If the capsule axis is parallel with the normal,
        // then the contacts collapse to one point and we can use the initially computed depth.
        // In this case, both contact positions should be extremely close together anyway.
        let collapse = local_normal_dot_depth_normal.absf().lt(EPS_7);
        manifold.depth[0] = depth.select(collapse, manifold.depth[0]);
        manifold.contact_exists[0] = (manifold.depth[0]).ge(negative_margin) & (!inactive_lanes);
        manifold.contact_exists[1] = ((contact_count.eq(i32x4::TWO) & !collapse)
            & manifold.depth[1].ge(negative_margin))
            & (!inactive_lanes);

        manifold.normal = contact_context
            .orientation_b
            .mul_vec3(local_normal)
            .as_unit_vec3x4_unchecked();
        manifold.offset_a[0] = contact_context.orientation_b.mul_vec3(contact_0);
        manifold.offset_a[1] = contact_context.orientation_b.mul_vec3(contact_1);
        manifold.offset_a[0] += contact_context.offset_b;
        manifold.offset_a[1] += contact_context.offset_b;
        manifold.feature_id[0] = u32x4::ZERO;
        manifold.feature_id[1] = u32x4::ONE;

        // Adjust offsets to shape surface
        manifold.offset_a[0] += -manifold.normal * manifold.depth[0];
        manifold.offset_a[1] += -manifold.normal * manifold.depth[1];
    }
}

/// Input and output are all in b local space.
/// Return `(contact_t_min, contact_t_max)` which are 2 contact lengths on segment b.
/// Note: Input and compute are all in b local space
#[inline]
pub(crate) fn get_contact_interval_between_segments(
    a_half_length: f32x4,
    b_half_length: f32x4,
    axis_a: &Vec3x4,
    local_normal: &Vec3x4,
    inverse_horizontal_normal_length_squared_b: f32x4,
    offset_a_to_b: &Vec3x4,
) -> (f32x4, f32x4) {
    const LOWER_THRESHOLD_ANGLE: f32 = 0.02;
    const UPPER_THRESHOLD_ANGLE: f32 = 0.15;
    const LOWER_THRESHOLD: f32 = LOWER_THRESHOLD_ANGLE * LOWER_THRESHOLD_ANGLE;
    const UPPER_THRESHOLD: f32 = UPPER_THRESHOLD_ANGLE * UPPER_THRESHOLD_ANGLE;

    let (_, _, _, lb, lb_min, lb_max) =
        get_closest_points_between_segments(axis_a, offset_a_to_b, a_half_length, b_half_length);
    // Get the plane_normal of the plane defined b and closest_b_to_a.
    // Only when the angle is small enough, we consider the two capsules are almost coplanar,
    // and interaction now could be a line(diff min,max_closest_point_on_a), otherwise they are
    // same points.
    // angle ~= sin(angle) = ra.dot(rb x normal)/||rb x normal||)
    // angle^2 ~= (ra.dot(rb x normal))^2 / ||rb x normal||^2
    // rb x normal = (normal.z, -normal.x) since rb is (0,1,0)
    let dot = axis_a.x * local_normal.z - axis_a.z * local_normal.x;
    let squared_angle = dot * dot * inverse_horizontal_normal_length_squared_b;

    // Angle close to UPPER is close to 0, and angle close to LOWER is close to 1,
    // which means the smaller angle has larger contact line.
    let interval_weight = ((f32x4::splat(UPPER_THRESHOLD) - squared_angle)
        * f32x4::splat((UPPER_THRESHOLD - LOWER_THRESHOLD).recip()))
    .clamp(f32x4::ZERO, f32x4::ONE);
    // When squared_angle >= UPPER, interval_weight = 0, a_min == a_max == la.
    let weighted_lb = lb - lb * interval_weight;
    let contact_t_min = interval_weight * lb_min + weighted_lb;
    let contact_t_max = interval_weight * lb_max + weighted_lb;
    (contact_t_min, contact_t_max)
}

/// Find a point in cylinder that is closest to the input point.
fn get_closest_point_between_point_and_cylinder(
    point: &Vec3x4, // b local
    b: &CylinderWide,
    radius_squared: f32x4,
) -> Vec3x4 {
    let distance_to_y_axis = point.x * point.x + point.z * point.z;
    let need_horizontal_clamp = distance_to_y_axis.gt(radius_squared);
    let clamp_scale = b.radius / distance_to_y_axis.sqrtf();
    // If the input point is outside the cylinder, clamp it to the cylinder surface which is the
    // closest point.
    Vec3x4::new(
        (clamp_scale * point.x).select(need_horizontal_clamp, point.x),
        point.y.clamp(-b.half_height, b.half_height),
        (clamp_scale * point.z).select(need_horizontal_clamp, point.z),
    )
}

/// Find the closest point pair between line segment and the cylinder.
/// Return offset as closest cylinder point to line segment point, intersected return zero
/// Note: input and output are all in b local space
fn get_closest_offset_between_line_segment_and_cylinder(
    offset_a: &Vec3x4,
    direction_a: &Vec3x4,
    a_half_length: f32x4,
    b: &CylinderWide,
    mut inactive_lanes: bool32x4,
) -> Vec3x4 {
    let mut min = -a_half_length;
    let mut max = a_half_length;
    // The point on line segment is parameterized as origin + direction * length.
    let mut length = f32x4::ZERO;
    let radius_squared = b.radius * b.radius;
    let origin_dot = direction_a.dot(offset_a);
    let epsilon = a_half_length * EPS_7;
    // Iterator the point on the line segment to find the closest one to the cylinder.
    for _ in 0..12 {
        let line_point = offset_a + direction_a * length;
        let cylinder_point =
            get_closest_point_between_point_and_cylinder(&line_point, b, radius_squared);
        // Project cylinder_point back to the line segment, we get the closest point to
        // cylinder_point. This closest point provide a iterator direction.
        let new_length = (cylinder_point.dot(direction_a) - origin_dot).clamp(min, max);
        let change = new_length - length;
        inactive_lanes = inactive_lanes | change.absf().lt(epsilon);
        if inactive_lanes.all() {
            // Finish the loop if no change.
            break;
        }
        // Reduce the range of the line segment towards the closest point.
        let moved_up = change.gt(f32x4::ZERO);
        min = new_length.select(moved_up, min);
        max = max.select(moved_up, new_length);
        let new_length = (min + max) * f32x4::HALF;
        // Update for only active lanes
        length = length.select(inactive_lanes, new_length);
    }

    let line_point = direction_a * length + offset_a;
    let cylinder_point =
        get_closest_point_between_point_and_cylinder(&line_point, b, radius_squared);
    // If intersect, line_point == cylinder_point, return zero normal
    line_point - cylinder_point
}

/// Return: `(la, la_min, la_max, lb, lb_min, lb_max)`
/// la: length of point to origin of capsule segment
/// Note: Input and compute are all in b local space
#[inline(always)]
fn get_closest_points_between_segments(
    direction_a: &Vec3x4,
    offset_a_to_b: &Vec3x4, // b - a
    a_half_length: f32x4,
    b_half_length: f32x4,
) -> (f32x4, f32x4, f32x4, f32x4, f32x4, f32x4) {
    // Consider two line segments as two lines first.
    //
    // We want to minimize distance = ||(a + la * ra) - (b + lb * rb)||
    // The rotation ||ra|| == ||rb|| == 1 == ra^2 == rb^2
    // n = ra x rb, where n is the normal of the plane defined by the two lines.
    // ||n|| = ||ra|| * ||rb| * sin(θ) = sin(θ)
    //
    // la: the length from the closest point on line a to a center.
    // Project b-a to the normal of rb x n / ||n||, it also equals to la * sin(θ)
    // Now we get (b - a) * (rb x n) / ||n|| = la * sin(θ)
    // la = (b - a) * (rb x n) / ||n|| / sin(θ) = (b - a) * (rb x n) / ||n||^2
    // lb = la * (ra * rb) - rb * (b - a)
    //
    // rb x n = rb x (ra x rb) = (rb * rb) * ra - (rb * ra) * rb = ra - rb * (ra * rb)
    // ||n||^2 = ||ra x rb||^2 = ||ra||^2 * ||rb||^2 - (ra * rb)^2 = 1 - (ra * rb)^2
    //
    // la = ((b - a) * ra - (b - a) * rb * (ra * rb)) / (1 - (ra * rb)^2)

    // ra * (b - a)
    let ra_offset_b = direction_a.dot(offset_a_to_b);
    // rb * (b - a), in b local space rb = (0,1,0)
    let rb_offset_b = offset_a_to_b.y;
    // ra * rb
    let rarb = direction_a.y;
    let mut la = (ra_offset_b - rb_offset_b * rarb) / f32x4::max(EPS_15, f32x4::ONE - rarb * rarb);
    let mut lb = la * rarb - rb_offset_b;

    // Project each line segment onto the other line segment to clamp.
    // The projected intervals are:
    // B onto A: +-b_half_length * (ra * rb) + ra * offset_b
    // A onto B: +-a_half_length * (ra * rb) - rb * offset_b
    let abs_rarb = rarb.absf();
    let b_onto_a_offset = b_half_length * abs_rarb;
    let a_onto_b_offset = a_half_length * abs_rarb;
    let la_min = (ra_offset_b - b_onto_a_offset).clamp(-a_half_length, a_half_length);
    let la_max = (ra_offset_b + b_onto_a_offset).clamp(-a_half_length, a_half_length);
    let lb_min = (-a_onto_b_offset - rb_offset_b).clamp(-b_half_length, b_half_length);
    let lb_max = (a_onto_b_offset - rb_offset_b).clamp(-b_half_length, b_half_length);

    la = la.clamp(la_min, la_max);
    lb = lb.clamp(lb_min, lb_max);
    (la, la_min, la_max, lb, lb_min, lb_max)
}

/// Calibrate the normal and depth for deeply intersected, which is input as INFINITY and MAX.
/// First set depth alone b's local y axis for capsule intersect cylinder cap.
/// Then get the closest point pair between segments, and calibrate the depth, choose the shorter
/// one.
/// Note: Input and compute are all in b local space
fn calibrate_normal_and_depth(
    depth: &mut f32x4, /* distance from the line segment to the cylinder, outside is negative,
                        * intersect is Max. */
    local_normal: &mut Vec3x4,
    calibrate_info: &CalibrateInfo,
    a_half_height: f32x4,
    offset_a_to_b: &Vec3x4, // negative local_offset_a
    deeply_intersected: bool32x4,
) {
    // Product segment endpoint on cylinder's Y axis.
    let segment_endpoint_on_y = f32x4::absf(calibrate_info.local_offset_a.y)
        - f32x4::absf(calibrate_info.direction_a.y * a_half_height);
    // Inside is negative
    let endpoint_in_cylinder_on_y = calibrate_info.b.half_height - segment_endpoint_on_y;
    // When deeply_intersected, depth is MAX, use endpoint_in_cylinder_on_y to calibrate depth.
    // depth = intersect depth on local b's y axis
    *depth = endpoint_in_cylinder_on_y.select(deeply_intersected, *depth);
    // local_normal = (0, +/-1, 0)
    let normal_y = Vec3x4::lane_select(
        calibrate_info.local_offset_a.y.gt(f32x4::ZERO),
        Vec3x4::Y,
        Vec3x4::NEG_Y,
    );
    *local_normal = Vec3x4::lane_select(deeply_intersected, normal_y, *local_normal);

    let (la, _, _, lb, _, _) = get_closest_points_between_segments(
        calibrate_info.direction_a,
        offset_a_to_b,
        a_half_height,
        calibrate_info.b.half_height,
    );

    // Offset from the closest point on the line segment to the cylinder center(0,0,0)
    let mut offset = calibrate_info.direction_a * la + calibrate_info.local_offset_a;
    // Offset of the closest point pair between segments
    offset.y -= lb;
    // Normalize the offset of closest point b to closest point a
    let offset_normal = offset.normalize_or(Vec3x4::X, EPS_7);

    // Compute the depth along the offset_normal.
    let center_separation_along_normal = calibrate_info.local_offset_a.dot(offset_normal);
    let cylinder_contribution = f32x4::absf(calibrate_info.b.half_height * offset_normal.y)
        + calibrate_info.b.radius
            * f32x4::sqrtf(f32x4::max(
                f32x4::ZERO,
                f32x4::ONE - offset_normal.y * offset_normal.y,
            ));
    let capsule_axis_dot_normal = calibrate_info.direction_a.dot(offset_normal);
    let capsule_contribution = f32x4::absf(capsule_axis_dot_normal) * a_half_height;
    let depth_on_offset_normal =
        cylinder_contribution + capsule_contribution - center_separation_along_normal;

    // Select the shorter depth.
    let use_depth_on_offset_normal = deeply_intersected & (depth_on_offset_normal.lt(*depth));
    *depth = depth_on_offset_normal.select(use_depth_on_offset_normal, *depth);
    *local_normal = Vec3x4::lane_select(use_depth_on_offset_normal, offset_normal, *local_normal);
}

struct CalibrateInfo<'a> {
    direction_a: &'a Vec3x4,
    local_offset_a: &'a Vec3x4,
    b: &'a CylinderWide,
}
impl<'a> CalibrateInfo<'a> {
    fn new(
        direction_a: &'a Vec3x4,
        local_offset_a: &'a Vec3x4,
        b: &'a CylinderWide,
    ) -> CalibrateInfo<'a> {
        CalibrateInfo {
            direction_a,
            local_offset_a,
            b,
        }
    }
}

/// Note: Input and compute are all in b local space
fn calibrate_cap_contacts(
    contact_0: &mut Vec3x4,
    contact_1: &mut Vec3x4,
    contact_count: &mut i32x4,
    calibrate_info: &CalibrateInfo,
    local_normal: &Vec3x4,
    a_half_height: f32x4,
    cap_intersected: bool32x4,
) {
    let cap_height = calibrate_info.b.half_height.select(
        local_normal.y.gt(f32x4::ZERO),
        -calibrate_info.b.half_height,
    );
    let endpoint_offset = calibrate_info.direction_a * a_half_height;
    // Cylinder cap center(0,b.half_height,0) to capsule line segnemt endpoints
    let cap_center_to_endpoint_pos = Vec3x4::new(
        calibrate_info.local_offset_a.x + endpoint_offset.x,
        calibrate_info.local_offset_a.y + endpoint_offset.y - cap_height,
        calibrate_info.local_offset_a.z + endpoint_offset.z,
    );
    let cap_center_to_endpoint_neg = Vec3x4::new(
        calibrate_info.local_offset_a.x - endpoint_offset.x,
        calibrate_info.local_offset_a.y - endpoint_offset.y - cap_height,
        calibrate_info.local_offset_a.z - endpoint_offset.z,
    );

    let inverse_normal_y = local_normal.y.recip();
    let t_negative = cap_center_to_endpoint_neg.y * inverse_normal_y;
    let t_positive = cap_center_to_endpoint_pos.y * inverse_normal_y;
    let projected_pos = Vec2x4::new(
        cap_center_to_endpoint_pos.x - local_normal.x * t_positive,
        cap_center_to_endpoint_pos.z - local_normal.z * t_positive,
    );
    let projected_neg = Vec2x4::new(
        cap_center_to_endpoint_neg.x - local_normal.x * t_negative,
        cap_center_to_endpoint_neg.z - local_normal.z * t_negative,
    );

    // Project endpoints to cap circle
    let projected_offset = projected_pos - projected_neg;
    // ||a + ab * t|| = radius
    // dot(a + ab * t, a + ab * t) = radius * radius
    // dot(a,a) - radius * radius + t * 2 * dot(a, ab) + t^2 * dot(ab, ab) = 0
    let coefficient_c =
        projected_neg.dot(projected_neg) - calibrate_info.b.radius * calibrate_info.b.radius;
    let coefficient_b = projected_neg.dot(projected_offset);
    let coefficient_a = projected_offset.length_squared();
    let inverse_a = coefficient_a.recip();
    let t_offset = (coefficient_b * coefficient_b - coefficient_a * coefficient_c)
        .max(f32x4::ZERO)
        .sqrtf()
        * inverse_a;
    let t_base = -coefficient_b * inverse_a;
    let mut t_min = (t_base - t_offset).clamp(f32x4::ZERO, f32x4::ONE);
    let mut t_max = (t_base + t_offset).clamp(f32x4::ZERO, f32x4::ONE);

    let use_fallback = coefficient_a.lt(EPS_12);
    t_min = f32x4::ZERO.select(use_fallback, t_min);
    t_max = f32x4::ZERO.select(use_fallback, t_max);
    let cap_contact_0 = Vec3x4::new(
        t_min * projected_offset.x + projected_neg.x,
        cap_height,
        t_min * projected_offset.y + projected_neg.y,
    );
    let cap_contact_1 = Vec3x4::new(
        t_max * projected_offset.x + projected_neg.x,
        cap_height,
        t_max * projected_offset.y + projected_neg.y,
    );

    // Fixed epsilon- the t value scales an offset that is generally proportional to object sizes.
    let cap_contact_count = i32x4::TWO.select((t_max - t_min).gt(EPS_5), i32x4::ONE);
    *contact_count = cap_contact_count.select(cap_intersected, *contact_count);
    *contact_0 = Vec3x4::lane_select(cap_intersected, cap_contact_0, *contact_0);
    *contact_1 = Vec3x4::lane_select(cap_intersected, cap_contact_1, *contact_1);
}

impl_pair_narrowphase!(Capsule, Cylinder, CapsuleWide, CylinderWide, 2);