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

use super::common::{generate_interior_points, NormalizeExt, EPS_10, EPS_6, FRAC_1_SQRT_2};
use crate::collision_tasks::cuboid_cuboid_tester_helper::{
    Candidates, CuboidFace, CuboidFaceInitConfig, ReduceContext,
};
use crate::collision_tasks::cylinder_cylinder_tester::project_onto_cap_b;
use crate::collision_tasks::traits::Axis::{X, Y, Z};
use crate::collision_tasks::traits::TransformWide;
use crate::collision_tasks::{tootbird, ShapeWideTester};
use crate::convex_contact_manifold::{Convex4ContactManifoldWide, ManifoldCandidateWide};
use crate::shapes::{Cuboid, CuboidWide, Cylinder, CylinderWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairWideTest};
use crate::{ConvexContactManifold, PairTest, ShapeContainer, ShapeTester};

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

    // 4 manifold in Convex4ContactManifoldWide
    fn test(
        a: &CuboidWide,
        b: &CylinderWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        let pair_count_u32 =
            u32::try_from(contact_context.pair_count).expect("pair_count must in range");
        let rotation_b_inverse = contact_context.orientation_b.inverse();
        let a_2_b_transform = rotation_b_inverse * contact_context.orientation_a; // cuboid rotation in cylinder space
        let ra2b_matrix = Mat3x4::from_quat(a_2_b_transform);
        let rb = Mat3x4::from_quat(*contact_context.orientation_b);
        let local_offset_b = rotation_b_inverse * contact_context.offset_b;
        let local_offset_a = -local_offset_b; // cuboid center in cylinder space
        let local_normal = local_offset_a.normalize_or(Vec3x4::Y, EPS_10);

        let mut inactive_lanes = u32x4::splat(pair_count_u32).le(u32x4::from([0, 1, 2, 3]));
        let depth_threshold = -contact_context.speculative_margin;
        // the min between (max depth of cuboid) and (max_depth of cylinder)
        let epsilon_scale = a.half_length.max_element().min(b.half_height.max(b.radius));

        let toot_bird_result = tootbird::find_minimum_depth(
            b,
            a,
            &TransformWide::new(local_offset_a, &a_2_b_transform),
            local_normal,
            &tootbird::IterContext::new(
                inactive_lanes,
                epsilon_scale * EPS_6,
                depth_threshold,
                25,
                None,
                false,
            ),
        );
        inactive_lanes = inactive_lanes | (toot_bird_result.depth.lt(depth_threshold));
        if inactive_lanes.all() {
            // All lanes are either inactive or were found to have a depth lower than the
            // speculative margin, so we can just quit early.
            manifold.reset(4);
            return;
        }

        // We generate contacts according to the dominant features along the collision normal.
        // The possible pairs are:
        // Face A-Cap A
        // Face A-Side A

        let cuboid_face_config = CuboidFaceInitConfig {
            use_x_as_normal_tangent_x_y: [Y, Z],
            use_y_as_normal_tangent_x_y: [Z, X],
            use_z_as_normal_tangent_x_y: [X, Y],
        };
        let mut cuboid_face = CuboidFace::new(
            toot_bird_result.normal,
            &ra2b_matrix,
            &a.half_length,
            false,
            &cuboid_face_config,
        );

        cuboid_face.center += local_offset_a;

        // width, height, length <-> CuboidWide.half_length.(x, y, z)
        let vertices = cuboid_face.vertices();
        let v00 = vertices.v00;
        let v10 = vertices.v10;
        let v01 = vertices.v01;
        let v11 = vertices.v11;

        let cap_center_b_y =
            (-b.half_height).select(toot_bird_result.normal.y.lt(f32x4::ZERO), b.half_height);
        let use_cap = (toot_bird_result.normal.y.absf().gt(FRAC_1_SQRT_2)) & (!inactive_lanes);
        let face_normal_dot_local_normal = cuboid_face.normal.dot(toot_bird_result.normal);
        let inverse_face_normal_dot_local_normal = face_normal_dot_local_normal.recip();

        if use_cap.any() {
            // At least one lane needs a cap-face manifold.
            let mut candidates = Candidates::<12>::default();

            //Project the edges down onto the cap's plane.
            let inverse_local_normal_y = toot_bird_result.normal.y.recip();
            let p00 = project_onto_cap_b(
                cap_center_b_y,
                inverse_local_normal_y,
                toot_bird_result.normal,
                v00,
            );
            let p10 = project_onto_cap_b(
                cap_center_b_y,
                inverse_local_normal_y,
                toot_bird_result.normal,
                v10,
            );
            let p01 = project_onto_cap_b(
                cap_center_b_y,
                inverse_local_normal_y,
                toot_bird_result.normal,
                v01,
            );
            let p11 = project_onto_cap_b(
                cap_center_b_y,
                inverse_local_normal_y,
                toot_bird_result.normal,
                v11,
            );

            let mut intersect_results =
                [(f32x4::default(), f32x4::default(), bool32x4::default()); 4];
            let edges = [
                Edge::new(p00, p01),
                Edge::new(p10, p00),
                Edge::new(p01, p11),
                Edge::new(p11, p10),
            ];
            for (edge, intersect_result) in edges.iter().zip(intersect_results.iter_mut()) {
                *intersect_result = intersect_line_circle(edge.start, edge.direction, b.radius);
            }
            for intersect_result in &mut intersect_results {
                intersect_result.0 = intersect_result.0.clamp(f32x4::ZERO, f32x4::ONE);
                intersect_result.1 = intersect_result.1.clamp(f32x4::ZERO, f32x4::ONE);
            }

            for (i, (intersect_result, edge)) in
                intersect_results.iter_mut().zip(edges.iter()).enumerate()
            {
                let intersected = intersect_result.2;
                let allow_contacts = intersected & use_cap;
                try_add_edge(
                    edge,
                    intersect_result.0,
                    intersect_result.1,
                    allow_contacts,
                    u32x4::splat(i as u32),
                    contact_context.pair_count,
                    &mut candidates,
                );
            }
            let (interior_0, interior_1, interior_2, interior_3) = generate_interior_points(
                b,
                toot_bird_result.normal.as_vec3x4(),
                toot_bird_result.closest_point_on_a,
            );

            // --o
            //
            //          -a--------d
            //          |    x     |
            //          |         |
            //         -b--------c
            // to check x is inside the rectangle abcd,o is the origin
            // we obtain <ob^ba> to get the scaled distance from o to ab,that is the min distance in
            // x direction
            // and <oc^ba> to get the scaled distance from o to cd,that is the max distance in x
            // direction
            // and <ob^bc> to get the scaled distance  from o to bc,that is the min distance in y
            // direction
            // and <od^bc> to get the scaled distance from o to ad,that is the max distance in y
            // direction
            // if x is inside the rectangle,then the scaled distance in x direction and y direction
            // should be in the range [min,max] above
            let ab = edges[0].direction;
            let bc = edges[1].direction;
            let ob = p00;
            let oc = p10;
            let od = p11;
            let signed_distance_x0 = ob.perp_dot(ab);
            let signed_distance_x1 = oc.perp_dot(ab);
            let signed_distance_y0 = ob.perp_dot(bc);
            let signed_distance_y1 = od.perp_dot(bc);
            let signed_distance_x_min = signed_distance_x0.min(signed_distance_x1);
            let signed_distance_x_max = signed_distance_x0.max(signed_distance_x1);
            let signed_distance_y_min = signed_distance_y0.min(signed_distance_y1);
            let signed_distance_y_max = signed_distance_y0.max(signed_distance_y1);

            let edge_clip_info_ab = EdgeClipPointsInfo {
                edge: ab,
                signed_distance_min: signed_distance_x_min,
                signed_distance_max: signed_distance_x_max,
            };

            let edge_clip_info_bc = EdgeClipPointsInfo {
                edge: bc,
                signed_distance_min: signed_distance_y_min,
                signed_distance_max: signed_distance_y_max,
            };

            [interior_0, interior_1, interior_2, interior_3]
                .iter()
                .zip([8, 9, 10, 11].iter())
                .for_each(
                    #[inline]
                    |(interior, feature_id)| {
                        try_add_point(
                            *interior,
                            u32x4::splat(*feature_id),
                            &edge_clip_info_ab,
                            &edge_clip_info_bc,
                            use_cap,
                            &mut candidates,
                            contact_context.pair_count,
                        );
                    },
                );

            let cap_center_to_cuboid_face_center = Vec3x4::new(
                cuboid_face.center.x,
                cuboid_face.center.y - cap_center_b_y,
                cuboid_face.center.z,
            );
            let tangent_b_x = Vec3x4::new(f32x4::ONE, f32x4::ZERO, f32x4::ZERO);
            let tangent_b_y = Vec3x4::new(f32x4::ZERO, f32x4::ZERO, f32x4::ONE);

            let reduce_context = ReduceContext {
                face_a_normal: &cuboid_face.normal,
                b_center_to_a_center: &cap_center_to_cuboid_face_center,
                face_b_tangent_x: &tangent_b_x,
                face_b_tangent_y: &tangent_b_y,
            };
            candidates.reduce(
                &reduce_context,
                depth_threshold,
                epsilon_scale,
                contact_context.pair_count,
                &toot_bird_result.normal,
                &mut manifold.contact_exists,
            );

            let mut local_contact = Vec3x4::new(f32x4::ZERO, cap_center_b_y, f32x4::ZERO);
            for (candidate, (offset_a, (feature_id, depth))) in candidates.value.iter().take(4).zip(
                manifold.offset_a.iter_mut().zip(
                    manifold
                        .feature_id
                        .iter_mut()
                        .zip(manifold.depth.iter_mut()),
                ),
            ) {
                local_contact.x = candidate.x;
                local_contact.z = candidate.y;
                let a_to_local_contact = local_contact + local_offset_b;
                *offset_a = rb * (a_to_local_contact);
                *feature_id = candidate.feature_id;
                *depth = candidate.depth;
            }
        } else {
            manifold.contact_exists = [bool32x4::FALSE; 4];
        }

        let use_side = (!use_cap) & (!inactive_lanes);
        if use_side.any() {
            const LOWER_THRESHOLD_ANGLE: f32 = 0.01;
            const UPPER_THRESHOLD_ANGLE: f32 = 0.02;
            const LOWER_THRESHOLD: f32 = LOWER_THRESHOLD_ANGLE * LOWER_THRESHOLD_ANGLE;
            const UPPER_THRESHOLD: f32 = UPPER_THRESHOLD_ANGLE * UPPER_THRESHOLD_ANGLE;

            let edge_normal_x = cuboid_face.tangent_x.cross(toot_bird_result.normal); // Points up
            let edge_normal_y = cuboid_face.tangent_y.cross(toot_bird_result.normal); // Points right

            // Center of the side line is just (closestOnB.X, 0, closestOnB.Z), sideLineDirection is
            // just (0, 1, 0).
            // t = dot(sideLineStart - pointOnFaceEdge, edgeNormal) / dot(sideLineDirection,
            // edgeNormal)
            let x_denominator = edge_normal_x.y.recip();
            let y_denominator = edge_normal_y.y.recip();
            let edge_normal_x_length_squared = edge_normal_x.length_squared();
            let edge_normal_y_length_squared = edge_normal_y.length_squared();
            let inverse_edge_normal_x_length_squared = edge_normal_x_length_squared.recip();
            let inverse_edge_normal_y_length_squared = edge_normal_y_length_squared.recip();
            let side_line_to_v00 = Vec3x4::new(
                v00.x - toot_bird_result.closest_point_on_a.x,
                v00.y,
                v00.z - toot_bird_result.closest_point_on_a.z,
            );
            let side_line_to_v11 = Vec3x4::new(
                v11.x - toot_bird_result.closest_point_on_a.x,
                v11.y,
                v11.z - toot_bird_result.closest_point_on_a.z,
            );

            let bottom_numerator = edge_normal_x.dot(side_line_to_v00);
            let left_numerator = edge_normal_y.dot(side_line_to_v00);
            let top_numerator = edge_normal_x.dot(side_line_to_v11);
            let right_numerator = edge_normal_y.dot(side_line_to_v11);
            let x_invalid = edge_normal_x.y.eq(f32x4::ZERO);
            let y_invalid = edge_normal_y.y.eq(f32x4::ZERO);
            let min_value = f32x4::MIN;
            let max_value = f32x4::MAX;
            let t_x0 = bottom_numerator * x_denominator;
            let t_x1 = top_numerator * x_denominator;
            let t_y0 = left_numerator * y_denominator;
            let t_y1 = right_numerator * y_denominator;

            let interpolation_min = f32x4::splat(UPPER_THRESHOLD);
            let inverse_interpolation_span =
                f32x4::splat((UPPER_THRESHOLD - LOWER_THRESHOLD).recip());
            let unrestrict_weight_x = f32x4::max(
                f32x4::ZERO,
                f32x4::min(
                    f32x4::ONE,
                    (interpolation_min
                        - edge_normal_x.y * edge_normal_x.y * inverse_edge_normal_x_length_squared)
                        * inverse_interpolation_span,
                ),
            );
            let unrestrict_weight_y = f32x4::max(
                f32x4::ZERO,
                f32x4::min(
                    f32x4::ONE,
                    (interpolation_min
                        - edge_normal_y.y * edge_normal_y.y * inverse_edge_normal_y_length_squared)
                        * inverse_interpolation_span,
                ),
            );
            let regular_weight_x = f32x4::ONE - unrestrict_weight_x;
            let regular_weight_y = f32x4::ONE - unrestrict_weight_y;
            let negative_half_length = -b.half_height;
            let t_x_min = min_value.select(
                x_invalid,
                unrestrict_weight_x * negative_half_length
                    + regular_weight_x * f32x4::min(t_x0, t_x1),
            );
            let t_x_max = max_value.select(
                x_invalid,
                unrestrict_weight_x * b.half_height + regular_weight_x * f32x4::max(t_x0, t_x1),
            );
            let t_y_min = min_value.select(
                y_invalid,
                unrestrict_weight_y * negative_half_length
                    + regular_weight_y * f32x4::min(t_y0, t_y1),
            );
            let t_y_max = max_value.select(
                y_invalid,
                unrestrict_weight_y * b.half_height + regular_weight_y * f32x4::max(t_y0, t_y1),
            );
            // Shouldn't need to make contact generation conditional here.
            // The closest points are guaranteed to be on these chosen features;
            // they might just be in the same spot. We do clamp for numerical reasons.
            let t_max = f32x4::min(
                f32x4::max(negative_half_length, f32x4::min(t_x_max, t_y_max)),
                b.half_height,
            );
            let t_min = f32x4::min(
                f32x4::max(negative_half_length, f32x4::max(t_x_min, t_y_min)),
                b.half_height,
            );

            let local_contact0 = Vec3x4::new(
                toot_bird_result.closest_point_on_a.x,
                t_min,
                toot_bird_result.closest_point_on_a.z,
            );
            let local_contact1 = Vec3x4::new(
                toot_bird_result.closest_point_on_a.x,
                t_max,
                toot_bird_result.closest_point_on_a.z,
            );
            let contact0 = rb * local_contact0 + contact_context.offset_b;
            let contact1 = rb * local_contact1 + contact_context.offset_b;
            manifold.offset_a[0] = Vec3x4::lane_select(use_side, contact0, manifold.offset_a[0]);
            manifold.offset_a[1] = Vec3x4::lane_select(use_side, contact1, manifold.offset_a[1]);
            manifold.feature_id[0] = u32x4::ZERO.select(use_side, manifold.feature_id[0]);
            manifold.feature_id[1] = u32x4::ONE.select(use_side, manifold.feature_id[1]);
            let cuboid_face_to_contact0 = local_contact0 - cuboid_face.center;
            let cuboid_face_to_contact1 = local_contact1 - cuboid_face.center;
            let contact0_dot = cuboid_face_to_contact0.dot(cuboid_face.normal);
            let contact1_dot = cuboid_face_to_contact1.dot(cuboid_face.normal);
            let depth0 = contact0_dot * inverse_face_normal_dot_local_normal;
            let depth1 = contact1_dot * inverse_face_normal_dot_local_normal;
            manifold.depth[0] = depth0.select(use_side, manifold.depth[0]);
            manifold.depth[1] = depth1.select(use_side, manifold.depth[1]);
            manifold.contact_exists[0] = depth0
                .ge(depth_threshold)
                .select(use_side, manifold.contact_exists[0]);
            manifold.contact_exists[1] = (depth1.ge(depth_threshold) & t_max.gt(t_min))
                .select(use_side, manifold.contact_exists[1]);
            manifold.contact_exists[2] =
                bool32x4::FALSE.select(use_side, manifold.contact_exists[2]);
            manifold.contact_exists[3] =
                bool32x4::FALSE.select(use_side, manifold.contact_exists[3]);
        }

        manifold.normal = (rb * toot_bird_result.normal).as_unit_vec3x4_unchecked();

        // adjust `offset_a` to shape `a` surface
        for i in 0..manifold.offset_a.len() {
            manifold.offset_a[i] += manifold.normal * (-manifold.depth[i]);
        }
    }
}

#[inline(always)]
#[must_use]
pub fn intersect_line_circle(
    line_position: Vec2x4,
    line_direction: Vec2x4,
    radius: f32x4,
) -> (f32x4, f32x4, bool32x4) {
    // ||linePosition + lineDirection * t|| = radius
    // dot(linePosition + lineDirection * t, linePosition + lineDirection * t) = radius * radius
    // dot(linePosition, linePosition) - radius * radius
    // + t * 2 * dot(linePosition, lineDirection)
    // + t^2 * dot(lineDirection, lineDirection) = 0
    let a = line_direction.dot(line_direction).max(f32x4::splat(2e-38));
    let inverse_a = f32x4::ONE / a;
    let b = line_position.dot(line_direction);
    let c = line_position.dot(line_position) - radius * radius;
    let d = b * b - a * c;
    let intersected = d.ge(f32x4::ZERO);
    let t_offset = d.max(f32x4::ZERO).sqrtf() * inverse_a;
    let t_base = -b * inverse_a;
    let t_min = t_base - t_offset;
    let t_max = t_base + t_offset;
    (t_min, t_max, intersected)
}

fn try_add_edge(
    edge: &Edge,
    t_min: f32x4,
    t_max: f32x4,
    allow_contacts: bool32x4,
    edge_id: u32x4,
    pair_count: usize,
    candidates: &mut Candidates<12>,
) {
    let mut candidate = ManifoldCandidateWide {
        x: edge.start.x + edge.direction.x * t_min,
        y: edge.start.y + edge.direction.y * t_min,
        depth: f32x4::ZERO,
        feature_id: edge_id,
    };

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

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

fn try_add_point(
    point: Vec2x4,
    feature_id: u32x4,
    edge_0010: &EdgeClipPointsInfo,
    edge_1011: &EdgeClipPointsInfo,
    allow_contact: bool32x4,
    candidates: &mut Candidates<12>,
    pair_count: usize,
) {
    let edge_0010_dot = point.x * edge_0010.edge.y - point.y * edge_0010.edge.x;
    let edge_1011_dot = point.x * edge_1011.edge.y - point.y * edge_1011.edge.x;
    let contained = allow_contact
        & (edge_0010_dot.ge(edge_0010.signed_distance_min))
        & (edge_0010_dot.le(edge_0010.signed_distance_max))
        & (edge_1011_dot.ge(edge_1011.signed_distance_min))
        & (edge_1011_dot.le(edge_1011.signed_distance_max));
    let candidate = ManifoldCandidateWide {
        x: point.x,
        y: point.y,
        feature_id,
        depth: f32x4::ZERO,
    };
    candidates.add(&candidate, contained, pair_count);
}
struct Edge {
    start: Vec2x4,
    direction: Vec2x4,
}
impl Edge {
    fn new(start: Vec2x4, end: Vec2x4) -> Self {
        Self {
            start,
            direction: end - start,
        }
    }
}

struct EdgeClipPointsInfo {
    pub edge: Vec2x4,
    pub signed_distance_min: f32x4,
    pub signed_distance_max: f32x4,
}
impl_pair_narrowphase!(Cuboid, Cylinder, CuboidWide, CylinderWide, 4);