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::{vec2x4, vec3x4, Dot, Mat3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4};

use super::common::{EPS_10, EPS_15, EPS_3};
use crate::collision_tasks::{ShapeTester, ShapeWideTester};
use crate::convex_contact_manifold::{Convex4ContactManifoldWide, ConvexContactManifold};
use crate::shapes::{Capsule, CapsuleWide, Cuboid, CuboidWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairTest, PairWideTest};
use crate::ShapeContainer;

impl PairWideTest<CapsuleWide, CuboidWide> for ShapeWideTester {
    // 2 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &CapsuleWide,
        b: &CuboidWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        // Note: The following computations are all in cuboid local space.

        let rb = Mat3x4::from_quat(*contact_context.orientation_b);
        let to_local_b = rb.transpose();
        let local_offset_a = -(to_local_b * contact_context.offset_b);
        let orientation_a_in_local_b =
            to_local_b * Mat3x4::from_quat(*contact_context.orientation_a);
        let capsule_y_axis = orientation_a_in_local_b.y_axis;

        // Find the closest_point on the capsule segment to the cuboid center.
        let length_b_on_a_segment = local_offset_a
            .dot(capsule_y_axis)
            .clamp(-a.half_height, a.half_height);
        let closest_point = capsule_y_axis * length_b_on_a_segment;
        let offset_b_to_a_closest = local_offset_a - closest_point;

        // Find the closest_vertex of cuboid.
        let closest_vertex = Vec3x4::select(
            Vec3x4::cmplt(offset_b_to_a_closest, Vec3x4::ZERO),
            -b.half_length,
            b.half_length,
        );

        // We have a function to test capsule segment vs edge of closest_vertex alone z,
        // rotate the axis to reuse it.

        // XYZ -> YZX
        let (mut la, mut depth, yzx_normal) = test_cuboid_edge_z(
            to_yzx(&local_offset_a),
            to_yzx(&capsule_y_axis),
            to_yzx(&b.half_length),
            &a.half_height,
            &closest_vertex.y,
            &closest_vertex.z,
        );
        let mut local_normal = from_yzx(&yzx_normal);

        // XYZ -> ZXY
        let (new_la, new_depth, zxy_normal) = test_cuboid_edge_z(
            to_zxy(&local_offset_a),
            to_zxy(&capsule_y_axis),
            to_zxy(&b.half_length),
            &a.half_height,
            &closest_vertex.z,
            &closest_vertex.x,
        );
        select_depth_la_local_normal(
            &mut depth,
            &mut la,
            &mut local_normal,
            &new_depth,
            &new_la,
            from_zxy(&zxy_normal),
        );
        // XYZ -> XYZ
        let (new_la, new_depth, xyz_normal) = test_cuboid_edge_z(
            local_offset_a,
            capsule_y_axis,
            b.half_length,
            &a.half_height,
            &closest_vertex.x,
            &closest_vertex.y,
        );
        select_depth_la_local_normal(
            &mut depth,
            &mut la,
            &mut local_normal,
            &new_depth,
            &new_la,
            xyz_normal,
        );

        // Face X
        let (x_sign, new_depth) = test_cuboid_face(
            &local_offset_a.x,
            &capsule_y_axis.x,
            &a.half_height,
            &b.half_length.x,
        );
        select_depth_local_normal(
            &mut depth,
            &mut local_normal,
            &new_depth,
            vec3x4(x_sign, f32x4::ZERO, f32x4::ZERO),
        );
        // Face Y
        let (y_sign, new_depth) = test_cuboid_face(
            &local_offset_a.y,
            &capsule_y_axis.y,
            &a.half_height,
            &b.half_length.y,
        );
        select_depth_local_normal(
            &mut depth,
            &mut local_normal,
            &new_depth,
            vec3x4(f32x4::ZERO, y_sign, f32x4::ZERO),
        );
        // Face Z
        let (z_sign, new_depth) = test_cuboid_face(
            &local_offset_a.z,
            &capsule_y_axis.z,
            &a.half_height,
            &b.half_length.z,
        );
        select_depth_local_normal(
            &mut depth,
            &mut local_normal,
            &new_depth,
            vec3x4(f32x4::ZERO, f32x4::ZERO, z_sign),
        );

        // Select a axis closest to local_normal.
        let x_dot = local_normal.x * x_sign;
        let y_dot = local_normal.y * y_sign;
        let z_dot = local_normal.z * z_sign;
        let use_x = x_dot.gt(y_dot.max(z_dot));
        let use_y = y_dot.gt(z_dot) & !use_x;
        let use_z = !use_x & !use_y;

        // Unproject the capsule center and capsule rotation onto the selected face plane.
        let face_normal_dot_local_normal = x_dot.select(use_x, y_dot.select(use_y, z_dot));
        let inverse_face_normal_dot_local_normal = EPS_15.max(face_normal_dot_local_normal).recip();
        let capsule_rotation_dot_face_normal = (capsule_y_axis.x * x_sign).select(
            use_x,
            (capsule_y_axis.y * y_sign).select(use_y, capsule_y_axis.z * z_sign),
        );
        let capsule_center_dot_face_normal = (local_offset_a.x * x_sign).select(
            use_x,
            (local_offset_a.y * y_sign).select(use_y, local_offset_a.z * z_sign),
        );
        let face_plane_offset = b
            .half_length
            .x
            .select(use_x, b.half_length.y.select(use_y, b.half_length.z));
        let t_axis = capsule_rotation_dot_face_normal * inverse_face_normal_dot_local_normal;
        let t_center = (capsule_center_dot_face_normal - face_plane_offset)
            * inverse_face_normal_dot_local_normal;

        // Work in tangent space.
        // Face X uses tangents Y and Z.
        // Face Y uses tangents X and Z.
        // Face Z uses tangents X and Y.
        let rotation_offset = local_normal * t_axis;
        let center_offset = local_normal * t_center;
        let unprojected_axis = capsule_y_axis - rotation_offset;
        let unprojected_center = local_offset_a - center_offset;
        let tangent_space_axis = vec2x4(
            unprojected_axis.y.select(use_x, unprojected_axis.x),
            unprojected_axis.y.select(use_z, unprojected_axis.z),
        );
        let tangent_space_center = vec2x4(
            unprojected_center.y.select(use_x, unprojected_center.x),
            unprojected_center.y.select(use_z, unprojected_center.z),
        );
        // Slightly boost the size of the face to avoid minor numerical issues that could block
        // coplanar contacts.
        let epsilon_scale = b
            .half_length
            .x
            .max(b.half_length.y)
            .max(b.half_length.z)
            .min(a.half_height.max(a.radius));
        let epsilon = epsilon_scale * EPS_3;
        let half_extent_x = epsilon + b.half_length.y.select(use_x, b.half_length.x);
        let half_extent_y = epsilon + b.half_length.y.select(use_z, b.half_length.z);

        // Compute interval bounded by edge normals pointing along tangentX.
        let inverse_axis_x = -tangent_space_axis.x.recip();
        let inverse_axis_y = -tangent_space_axis.y.recip();
        let t_x0 = (tangent_space_center.x - half_extent_x) * inverse_axis_x;
        let t_x1 = (tangent_space_center.x + half_extent_x) * inverse_axis_x;
        let t_y0 = (tangent_space_center.y - half_extent_y) * inverse_axis_y;
        let t_y1 = (tangent_space_center.y + half_extent_y) * inverse_axis_y;
        let mut min_x = t_x0.min(t_x1);
        let mut max_x = t_x0.max(t_x1);
        let mut min_y = t_y0.min(t_y1);
        let mut max_y = t_y0.max(t_y1);
        // Protect against division by zero. If the unprojected capsule is within the slab, use an
        // infinite interval. If it's outside and parallel, use an invalid interval.
        let use_fall_back_x = tangent_space_axis.x.absf().lt(EPS_15);
        let use_fall_back_y = tangent_space_axis.y.absf().lt(EPS_15);
        let center_contained_x = tangent_space_center.x.absf().le(half_extent_x);
        let center_contained_y = tangent_space_center.y.absf().le(half_extent_y);
        let large_negative = f32x4::splat(-f32::MAX);
        let large_positive = f32x4::splat(f32::MAX);
        min_x = large_negative
            .select(center_contained_x, large_positive)
            .select(use_fall_back_x, min_x);
        max_x = large_positive
            .select(center_contained_x, large_negative)
            .select(use_fall_back_x, max_x);
        min_y = large_negative
            .select(center_contained_y, large_positive)
            .select(use_fall_back_y, min_y);
        max_y = large_positive
            .select(center_contained_y, large_negative)
            .select(use_fall_back_y, max_y);

        let face_min = min_x.max(min_y);
        let face_max = max_x.min(max_y);
        // Clamp to the capsule segment.
        let mut t_min = face_min.clamp(-a.half_height, a.half_height);
        let mut t_max = face_max.clamp(-a.half_height, a.half_height);
        let face_interval_exists = face_max.ge(face_min);
        t_min = t_min.min(la).select(face_interval_exists, la);
        t_max = t_max.max(la).select(face_interval_exists, la);

        // Each contact may have its own depth.
        // Imagine a face collision- if the capsule axis isn't fully parallel with the plane's
        // surface, it would be strange to use the same depth for both contacts. We have two
        // points on the capsule and cuboid. We can reuse the unprojeection from earlier to compute
        // the offset between them.
        let separation_min = t_center + t_axis * t_min;
        let separation_max = t_center + t_axis * t_max;
        manifold.depth[0] = a.radius - separation_min;
        manifold.depth[1] = a.radius - separation_max;

        // Convert length on capsule segment to b local offset.
        let local_a0 = capsule_y_axis * t_min;
        let local_a1 = capsule_y_axis * t_max;

        manifold.feature_id[0] = u32x4::ZERO;
        manifold.feature_id[1] = u32x4::ONE;

        // From b local space to world space.
        manifold.normal = (rb * local_normal).as_unit_vec3x4_unchecked();
        manifold.offset_a[0] = rb * local_a0;
        manifold.offset_a[1] = rb * local_a1;

        // Apply the normal offset to the contact positions.
        let negative_offset_from_a0 = -a.radius;
        let negative_offset_from_a1 = -a.radius;
        let normal_push0 = manifold.normal * negative_offset_from_a0;
        let normal_push1 = manifold.normal * negative_offset_from_a1;
        manifold.offset_a[0] += normal_push0;
        manifold.offset_a[1] += normal_push1;

        let minimum_accepted_depth = -contact_context.speculative_margin;
        manifold.contact_exists[0] = manifold.depth[0].ge(minimum_accepted_depth);
        manifold.contact_exists[1] = manifold.depth[1].ge(minimum_accepted_depth)
            & (t_max - t_min).gt(f32x4::EPSILON * a.half_height);
    }

    #[inline]
    fn should_reset_manifold_before_test() -> bool {
        false
    }
}

#[inline]
fn test_cuboid_face(
    offset_a_z: &f32x4,
    capsule_rotation_z: &f32x4,
    capsule_half_length: &f32x4,
    cuboid_half_length: &f32x4,
) -> (f32x4, f32x4) {
    let normal_sign = f32x4::ONE.select(offset_a_z.gt(f32x4::ZERO), f32x4::NEG_ONE);
    let depth = cuboid_half_length + capsule_rotation_z.absf() * capsule_half_length
        - normal_sign * offset_a_z;
    (normal_sign, depth)
}

#[inline]
fn select_depth_local_normal(
    depth: &mut f32x4,
    local_normal: &mut Vec3x4,
    new_depth: &f32x4,
    local_normal_candidate: Vec3x4,
) {
    let use_candidate = new_depth.lt(*depth);
    *depth = new_depth.select(use_candidate, *depth);
    *local_normal = Vec3x4::lane_select(use_candidate, local_normal_candidate, *local_normal);
}

#[inline]
fn select_depth_la_local_normal(
    depth: &mut f32x4,
    la: &mut f32x4,
    local_normal: &mut Vec3x4,
    new_depth: &f32x4,
    new_la: &f32x4,
    local_normal_candidate: Vec3x4,
) {
    let use_candidate = new_depth.lt(*depth);
    *la = new_la.select(use_candidate, *la);
    *depth = new_depth.select(use_candidate, *depth);
    *local_normal = Vec3x4::lane_select(use_candidate, local_normal_candidate, *local_normal);
}

/// When we have a vertex, select the edge with vertex alone z.
/// `cuboid_edge_center_x = vertex.x, cuboid_edge_center_y = vertex.y, cuboid_edge_center_z = 0`
/// Calculate the closest point on `a` segment to the edge.
/// Return `(la, closest_point_on_a, closest_normal)`
///     la: the length from the closest point on `a` segment to `a` center.
///
/// Note: This function is used to test the edge with vertex alone z, but we can also rotate the
/// axis to swap x or y to z to reuse this function.
#[inline]
fn inner_test_cuboid_edge_z(
    offset_a: Vec3x4,
    capsule_y_axis: Vec3x4,
    capsule_half_length: &f32x4,
    cuboid_edge_center_x: &f32x4,
    cuboid_edge_center_y: &f32x4,
    cuboid_half_length_z: &f32x4,
) -> (f32x4, Vec3x4, Vec3x4) {
    // About closest points between two lines, we can refer to: capsule_capsule_tester.rs
    // la = ((b - a) * ra - (b - a) * rb * (ra * rb)) / (1 - (ra * rb)^2)
    //     a: offset_a, b: cuboid_edge_center, ra: capsule_y_axis, rb: (0,0,1)

    // b.z == 0
    // (b - a) * ra = (b.x-a.x) * ra.x + (b.y-a.y) * ra.y + (0 - a.z) * ra.z
    let ra_offset_a_to_b = (cuboid_edge_center_x - offset_a.x) * capsule_y_axis.x
        + (cuboid_edge_center_y - offset_a.y) * capsule_y_axis.y
        - offset_a.z * capsule_y_axis.z;
    // (ra * rb) = ra.z
    // (b - a) * rb = (b.x-a.x) * 0 + (b.y-a.y) * 0 + (0 - a.z) * 1 = -a.z
    let mut la = (ra_offset_a_to_b + offset_a.z * capsule_y_axis.z)
        / EPS_15.max(f32x4::ONE - capsule_y_axis.z * capsule_y_axis.z);
    let mut lb = la * capsule_y_axis.z + offset_a.z;

    // Project segmenmt lines to each other to clamp la lb.
    let absrarb = capsule_y_axis.z.absf();
    let b_onto_a_offset = cuboid_half_length_z * absrarb;
    let a_onto_b_offset = capsule_half_length * absrarb;
    let la_min =
        (ra_offset_a_to_b - b_onto_a_offset).clamp(-capsule_half_length, *capsule_half_length);
    let la_max =
        (ra_offset_a_to_b + b_onto_a_offset).clamp(-capsule_half_length, *capsule_half_length);
    let b_min = (offset_a.z - a_onto_b_offset).clamp(-cuboid_half_length_z, *cuboid_half_length_z);
    let b_max = (offset_a.z + a_onto_b_offset).clamp(-cuboid_half_length_z, *cuboid_half_length_z);
    la = la.clamp(la_min, la_max);
    lb = lb.clamp(b_min, b_max);

    let closest_point_on_a = la * capsule_y_axis + offset_a;
    let closest_point_on_b = vec3x4(*cuboid_edge_center_x, *cuboid_edge_center_y, lb);
    let mut closest_normal_with_length = closest_point_on_a - closest_point_on_b;
    let mut squared_length = closest_normal_with_length.length_squared();

    // When length is 0, cuboid edge and capsule segment intersect, we use the normal of
    // (0,0,1) x capsule_y_axis, which is (-capsule_rotation_y, capsule_rotation_x, 0).
    let is_intersect = squared_length.lt(EPS_10);
    let intersect_normal = Vec3x4::new(-capsule_y_axis.y, capsule_y_axis.x, f32x4::ZERO);
    // When intersect and parallel, we use the normal of (1,0,0).
    let x_y_rotation = capsule_y_axis.y * capsule_y_axis.y + capsule_y_axis.x * capsule_y_axis.x;
    let is_parallel = is_intersect & x_y_rotation.lt(EPS_10);

    squared_length = f32x4::ONE.select(
        is_parallel,
        x_y_rotation.select(is_intersect, squared_length),
    );
    closest_normal_with_length =
        Vec3x4::lane_select(is_intersect, intersect_normal, closest_normal_with_length);
    closest_normal_with_length =
        Vec3x4::lane_select(is_parallel, Vec3x4::X, closest_normal_with_length);

    // `normal dot offset_a` to check should neg normal or not.
    let should_negate = closest_normal_with_length.dot(offset_a).lt(f32x4::ZERO);
    closest_normal_with_length = Vec3x4::lane_select(
        should_negate,
        -closest_normal_with_length,
        closest_normal_with_length,
    );

    let inverse_length = squared_length.sqrtf().recip();
    let closest_normal = closest_normal_with_length * inverse_length;

    (la, closest_point_on_a, closest_normal)
}

/// Return `(la, depth, normal)`
///     la: the length from the closest point on `a` segment to `a` center.
#[inline]
fn test_cuboid_edge_z(
    offset_a: Vec3x4,
    capsule_y_axis: Vec3x4,
    cuboid_half_length: Vec3x4,
    capsule_half_length: &f32x4,
    cuboid_vertex_x: &f32x4,
    cuboid_vertex_y: &f32x4,
) -> (f32x4, f32x4, Vec3x4) {
    // edge z center = (cuboid_vertex_x, cuboid_vertex_y, 0)
    let (la, closest_point_on_a, normal) = inner_test_cuboid_edge_z(
        offset_a,
        capsule_y_axis,
        capsule_half_length,
        cuboid_vertex_x,
        cuboid_vertex_y,
        &cuboid_half_length.z,
    );

    // Compute the depth along that normal.
    let cuboid_extreme = normal.abs().dot(cuboid_half_length);
    let capsule_extreme = normal.dot(closest_point_on_a);
    let depth = cuboid_extreme - capsule_extreme;
    (la, depth, normal)
}

#[inline]
fn to_yzx(v: &Vec3x4) -> Vec3x4 {
    Vec3x4::new(v.y, v.z, v.x)
}

#[inline]
fn to_zxy(v: &Vec3x4) -> Vec3x4 {
    Vec3x4::new(v.z, v.x, v.y)
}

#[inline]
fn from_yzx(v: &Vec3x4) -> Vec3x4 {
    Vec3x4::new(v.z, v.x, v.y)
}

#[inline]
fn from_zxy(v: &Vec3x4) -> Vec3x4 {
    Vec3x4::new(v.y, v.z, v.x)
}

impl_pair_narrowphase!(Capsule, Cuboid, CapsuleWide, CuboidWide, 2);