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, Bool, Float, Num, NumConstEx, PartialOrdEx, Signed};
use glam_det::{
    Dot, Isometry3x4, Mat3, Mat3x4, Point3, UnitQuat, UnitQuatx4, UnitVec3, Vec2, Vec2x4, Vec3,
    Vec3x4,
};

use super::capsule_convex_hull_tester::select_range_on_segment_vs_hull_face;
use super::common::{generate_interior_points, NormalizeExt, EPS_3, EPS_5, EPS_8, FRAC_1_SQRT_2};
use super::tootbird;
use crate::collision_tasks::manifold_candidate_helper::{
    CandidateScalarsReducer, ManifoldCandidateScalarSmallVec,
};
use crate::collision_tasks::traits::TransformWide;
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::{Convex4ContactManifoldWide, Face, ManifoldCandidate};
use crate::shapes::{ConvexHullWide, CylinderWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairWideTest};
use crate::{
    ConvexContactManifold, ConvexHull, ConvexHullId, Cylinder, PairTest, ShapeContainer,
    ShapeTester,
};

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

    // 4 manifold in Convex4ContactManifoldWide
    fn test(
        a: &CylinderWide,
        b: &ConvexHullWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        // Note: Following computes are all in b local space.

        let container = contact_context
            .complex_shape_container
            .expect("ShapeContainer is required for ConvexHullWide");
        let pair_count_i32 =
            i32::try_from(contact_context.pair_count).expect("pair_count must in range");
        let ra_in_world = Mat3x4::from_quat(*contact_context.orientation_a);
        let rb_in_world = Mat3x4::from_quat(*contact_context.orientation_b);
        let ra = rb_in_world.transpose() * ra_in_world;
        let shift_b = b.get_convexhull_shift_wide(container, contact_context.pair_count);
        let shift_b_world_space = rb_in_world.mul_vec3(shift_b);
        let center_b = b.get_center(container, contact_context.pair_count);
        let local_offset_a = -(rb_in_world.transpose() * contact_context.offset_b) - shift_b;
        let normal_a = (local_offset_a - center_b).normalize_or(Vec3x4::Y, EPS_8);
        let mut inactive_lanes = i32x4::splat(pair_count_i32).le(i32x4::from([0, 1, 2, 3]));

        let hull_epsilon_scale = b.estimate_epsilon_scale(inactive_lanes, container);
        let epsilon_scale = a.half_height.max(a.radius).min(hull_epsilon_scale);
        let depth_threshold = -contact_context.speculative_margin;
        let tootbird_result = tootbird::find_minimum_depth(
            b,
            a,
            &TransformWide::new(local_offset_a, &ra),
            normal_a,
            &tootbird::IterContext::new(
                inactive_lanes,
                epsilon_scale * EPS_5,
                depth_threshold,
                25,
                Some(container),
                false,
            ),
        );

        inactive_lanes = inactive_lanes | tootbird_result.depth.lt(depth_threshold);

        // If all lanes are inactive, return.
        if inactive_lanes.all() {
            return;
        }

        // Check whether cap vs hull or edge vs hull.
        let closest_a_to_closest_b = tootbird_result.normal * tootbird_result.depth;
        // tootbird_result.closest_point_on_a is closest_point_on_b.
        let closest_point_on_a = tootbird_result.closest_point_on_a - closest_a_to_closest_b;
        let local_normal_in_a = ra.transpose() * tootbird_result.normal;
        let cap_intersected = local_normal_in_a.y.absf().gt(FRAC_1_SQRT_2);

        // Prepare cap vs hull: cap center and interior points.
        let mut cap_center = Vec3x4::default();
        let mut interior_0 = Vec2x4::default();
        let mut interior_1 = Vec2x4::default();
        let mut interior_2 = Vec2x4::default();
        let mut interior_3 = Vec2x4::default();
        if (cap_intersected & !inactive_lanes).any() {
            let use_bottom = local_normal_in_a.y.gt(f32x4::ZERO);
            cap_center = ra.y_axis * (-a.half_height).select(use_bottom, a.half_height);
            cap_center += local_offset_a;

            let closest_on_a_to_a_center = closest_point_on_a - local_offset_a;
            let closest_in_a_local = ra.transpose() * closest_on_a_to_a_center;
            (interior_0, interior_1, interior_2, interior_3) =
                generate_interior_points(a, local_normal_in_a, closest_in_a_local);
        }

        // Prepare side edge vs hull: edge center.
        let mut cylinder_side_edge_center = Vec3x4::default();
        if !(cap_intersected | inactive_lanes).all() {
            let cylinder_to_closest_on_cylinder = closest_point_on_a - local_offset_a;
            let cylinder_local_closest_on_cylinder_y =
                cylinder_to_closest_on_cylinder.dot(ra.y_axis);
            let cylinder_edge_center_to_closest_on_cylinder =
                ra.y_axis * cylinder_local_closest_on_cylinder_y;
            cylinder_side_edge_center =
                closest_point_on_a - cylinder_edge_center_to_closest_on_cylinder;
        }

        let mut candidates_capacity = 4;
        for &b in b.iter_take(contact_context.pair_count) {
            // Cap cycle may have 2 contacts with each hull edge at most.
            // So we prepare 2 * vertices_count candidates.
            let vertices_count = container
                .get::<ConvexHull>(b)
                .expect("invalid shape id")
                .face_vertex_indices()
                .len();
            candidates_capacity = candidates_capacity.max(vertices_count * 2);
        }
        let mut candidates = ManifoldCandidateScalarSmallVec::with_capacity(candidates_capacity);
        let bounding_plane_epsilon = epsilon_scale * EPS_3;
        let transform_b = Isometry3x4::from_rotation_translation(
            *contact_context.orientation_b,
            *contact_context.offset_b,
        );

        let inverse_local_normal_dot_cap_normal = local_normal_in_a.y.recip();
        for (laned_index, _) in <[bool; 4]>::from(inactive_lanes)
            .into_iter()
            .take(contact_context.pair_count)
            .enumerate()
            .filter(|(_, inactive)| !inactive)
        {
            let hull_id = b.get_id(laned_index);
            let hull = container
                .get::<ConvexHull>(hull_id)
                .expect("invalid hull id");
            let laned_tootbird_normal = tootbird_result.normal.extract_lane(laned_index);
            let hull_face = hull.pick_representative_face(
                laned_tootbird_normal,
                // tootbird_result.closest_point_on_a is closest_point_on_b.
                Point3::from_vec3(tootbird_result.closest_point_on_a.extract_lane(laned_index)),
                bounding_plane_epsilon.extract(laned_index),
            );
            let face_vertex_indices = hull.get_vertex_indices(hull_face.face_index);
            let laned_hull_face_normal = hull_face.normal;
            let laned_ra = ra.extract_lane(laned_index);
            let last_index = face_vertex_indices[face_vertex_indices.len() - 1];
            let hull_face_origin = hull.get_point(last_index);

            // Cap vs hull.
            if cap_intersected.extract(laned_index) {
                candidates.clear();
                // We will test the interior points vs hull edges face and the cap circle vs hull
                // edges. Then we will reduce the contacts.
                let laned_cap_center = cap_center.extract_lane(laned_index);
                let laned_inverse_local_normal_dot_cap_normal =
                    inverse_local_normal_dot_cap_normal.extract(laned_index);

                let interior_0_lane = interior_0.extract_lane(laned_index);
                let interior_1_lane = interior_1.extract_lane(laned_index);
                let interior_2_lane = interior_2.extract_lane(laned_index);
                let interior_3_lane = interior_3.extract_lane(laned_index);
                let interior_points_x = f32x4::from([
                    interior_0_lane.x,
                    interior_1_lane.x,
                    interior_2_lane.x,
                    interior_3_lane.x,
                ]);
                let interior_points_y = f32x4::from([
                    interior_0_lane.y,
                    interior_1_lane.y,
                    interior_2_lane.y,
                    interior_3_lane.y,
                ]);
                let laned_radius = a.radius.extract(laned_index);

                let mut previous_index = last_index;
                let mut previous_vertex = project_onto_cap(
                    laned_cap_center,
                    &laned_ra,
                    laned_inverse_local_normal_dot_cap_normal,
                    laned_tootbird_normal.as_vec3(),
                    hull_face_origin.as_vec3(),
                );
                let mut is_interior_outside_hull = bool32x4::FALSE;

                for index in face_vertex_indices {
                    let hull_vertex = hull.get_point(*index);
                    let vertex = project_onto_cap(
                        laned_cap_center,
                        &laned_ra,
                        laned_inverse_local_normal_dot_cap_normal,
                        laned_tootbird_normal.as_vec3(),
                        hull_vertex.as_vec3(),
                    );

                    let hull_edge_offset = vertex - previous_vertex;
                    let previous_start_x = f32x4::splat(previous_vertex.x);
                    let previous_start_y = f32x4::splat(previous_vertex.y);
                    let hull_edge_offset_x = f32x4::splat(hull_edge_offset.x);
                    let hull_edge_offset_y = f32x4::splat(hull_edge_offset.y);
                    // Interior points vs hull edge plane.
                    // If the dot is positive, the point is outside the edge plane.
                    let mut interior_point_containment_dots =
                        (interior_points_x - previous_start_x) * hull_edge_offset_y
                            - (interior_points_y - previous_start_y) * hull_edge_offset_x;
                    // Bottom cap use negative.
                    if laned_inverse_local_normal_dot_cap_normal > 0.0 {
                        interior_point_containment_dots = -interior_point_containment_dots;
                    }
                    // When a interior is outside of one hull edge plane, it is outside of hull.
                    is_interior_outside_hull =
                        is_interior_outside_hull | interior_point_containment_dots.gt(f32x4::ZERO);

                    // Cap circle vs hull edge.
                    let (intersects, t_min, t_max) =
                        intersect_line_circle(previous_vertex, hull_edge_offset, laned_radius);
                    if intersects && candidates.len() < candidates_capacity {
                        let start_id = previous_index.index();
                        let end_id = index.index();
                        let base_feature_id = (start_id ^ end_id) << 8;

                        let tmp = hull_edge_offset * t_max + previous_vertex;
                        candidates.push(ManifoldCandidate::new(
                            tmp.x,
                            tmp.y,
                            base_feature_id + end_id,
                        ));

                        // If min is not equal to max, and min is greater than 0, save min.
                        if t_min < t_max && t_min > 0.0 && candidates.len() < candidates_capacity {
                            let tmp = hull_edge_offset * t_min + previous_vertex;
                            candidates.push(ManifoldCandidate::new(
                                tmp.x,
                                tmp.y,
                                base_feature_id + start_id,
                            ));
                        }
                    }

                    previous_index = *index;
                    previous_vertex = vertex;
                }

                for (i, _) in <[bool; 4]>::from(is_interior_outside_hull)
                    .into_iter()
                    .enumerate()
                    .filter(|(_, outside)| !outside)
                {
                    if candidates.len() == candidates_capacity {
                        break;
                    }
                    candidates.push(ManifoldCandidate::new(
                        interior_points_x.extract(i),
                        interior_points_y.extract(i),
                        i as u32,
                    ));
                }

                // We have tested all the interior points and cap circle vs hull edges, then we
                // should reduce the contacts.
                let laned_cylinder_face_x = laned_ra.x_axis;
                let laned_cylinder_face_y = laned_ra.z_axis;
                if !candidates.is_empty() {
                    let mut reducer = CandidateScalarsReducer::new(
                        &mut candidates,
                        manifold,
                        laned_index,
                        transform_b.extract_lane(laned_index),
                    );
                    let inverse_face_normal_a_dot_local_normal =
                        -1.0 / laned_tootbird_normal.dot(laned_hull_face_normal.as_vec3());
                    let dot_axis = laned_hull_face_normal * inverse_face_normal_a_dot_local_normal;
                    let cylinder_face = Face {
                        origin: Point3::from_vec3(laned_cap_center),
                        tangent_x: laned_cylinder_face_x.normalize_to_unit(),
                        tangent_y: laned_cylinder_face_y.normalize_to_unit(),
                    };
                    reducer.reduce(
                        dot_axis,
                        hull_face_origin,
                        &cylinder_face,
                        epsilon_scale.extract(laned_index),
                        -contact_context.speculative_margin.extract(laned_index),
                    );
                }
            } else {
                // Side edge vs hull, just like capsule segment vs hull.
                let laned_side_edge_half_length = a.half_height.extract(laned_index);
                let laned_side_edge_center = cylinder_side_edge_center.extract_lane(laned_index);
                let laned_cylinder_edge_axis = laned_ra.y_axis;
                let (
                    min_a_edge_to_b_edge,
                    min_direction_a_edge_dot_b_edge_plane_normal,
                    max_a_edge_to_b_edge,
                    max_direction_a_edge_dot_b_edge_plane_normal,
                ) = select_range_on_segment_vs_hull_face(
                    laned_cylinder_edge_axis,
                    laned_side_edge_center,
                    laned_side_edge_half_length,
                    laned_tootbird_normal,
                    hull,
                    face_vertex_indices,
                );

                let mut t_min = min_a_edge_to_b_edge / min_direction_a_edge_dot_b_edge_plane_normal;
                let mut t_max = max_a_edge_to_b_edge / max_direction_a_edge_dot_b_edge_plane_normal;
                let negated_edge_length = -laned_side_edge_half_length;
                t_min = t_min.clamp(negated_edge_length, laned_side_edge_half_length);
                t_max = t_max.clamp(negated_edge_length, laned_side_edge_half_length);

                let inverse_depth_denominator = laned_hull_face_normal
                    .dot(laned_tootbird_normal.as_vec3())
                    .recip();
                let laned_info = LanedInfo {
                    laned_side_edge_center,
                    laned_cylinder_edge_axis,
                    laned_hull_face_normal,
                    laned_rb: rb_in_world.extract_lane(laned_index),
                    laned_offset_b: contact_context.offset_b.extract_lane(laned_index),
                    laned_index,
                };

                insert_contact(
                    &laned_info,
                    t_max,
                    hull_face_origin.as_vec3(),
                    inverse_depth_denominator,
                    0,
                    0,
                    manifold,
                );
                if t_max - t_min > laned_side_edge_half_length * 1e-3 {
                    insert_contact(
                        &laned_info,
                        t_min,
                        hull_face_origin.as_vec3(),
                        inverse_depth_denominator,
                        1,
                        1,
                        manifold,
                    );
                } else {
                    manifold.contact_exists[1].replace(laned_index, false);
                }
                manifold.contact_exists[2].replace(laned_index, false);
                manifold.contact_exists[3].replace(laned_index, false);
            }
        }
        manifold.normal = (rb_in_world * tootbird_result.normal).as_unit_vec3x4_unchecked();
        for i in 0..manifold.offset_a.len() {
            if manifold.contact_exists[i].any() {
                manifold.offset_a[i] += shift_b_world_space;
            }
        }
    }
}

#[inline]
fn project_onto_cap(
    cap_center: Vec3,
    cylinder_orientation: &Mat3,
    inverse_local_normal_dot_a_y: f32,
    local_normal: Vec3,
    point: Vec3,
) -> Vec2 {
    let point_to_cap_center = cap_center - point;
    let t = point_to_cap_center.dot(cylinder_orientation.y_axis) * inverse_local_normal_dot_a_y;
    let projection_offset_b = local_normal * t;
    let projected_point = point - projection_offset_b;
    let cap_center_to_projected_point = projected_point - cap_center;
    Vec2::new(
        cap_center_to_projected_point.dot(cylinder_orientation.x_axis),
        cap_center_to_projected_point.dot(cylinder_orientation.z_axis),
    )
}

/// Returns `(intersects, t_min, t_max)` where `t_min` and `t_max` are the intervals for
/// intersection points on the line if intersects is true.
#[inline]
fn intersect_line_circle(
    line_position: Vec2,
    line_direction: Vec2,
    radius: f32,
) -> (bool, f32, f32) {
    let a = line_direction.dot(line_direction);
    let inverse_a = 1.0 / a;
    let b = line_position.dot(line_direction);
    let mut c = line_position.dot(line_position);
    let radius_squared = radius * radius;
    c -= radius_squared;
    let discriminant = b * b - a * c;
    if discriminant < 0.0 {
        return (false, 0.0, 0.0);
    }
    let t_offset = discriminant.sqrtf() * inverse_a;
    let t_base = -b * inverse_a;
    let mut t_min: f32;
    let mut t_max: f32;
    if a < 1e-12 && a > -1e-12 {
        t_min = t_base;
        t_max = t_base;
    } else {
        t_min = t_base - t_offset;
        t_max = t_base + t_offset;
    }
    if t_min < 0.0 {
        t_min = 0.0;
    }
    if t_max > 1.0 {
        t_max = 1.0;
    }
    (true, t_min, t_max)
}

#[derive(Clone)]
struct LanedInfo {
    laned_side_edge_center: Vec3,
    laned_cylinder_edge_axis: Vec3,
    laned_hull_face_normal: UnitVec3,
    laned_rb: Mat3,
    laned_offset_b: Vec3,
    laned_index: usize,
}

#[inline]
fn insert_contact(
    laned_info: &LanedInfo,
    t: f32,
    hull_face_origin: Vec3,
    inverse_depth_denominator: f32,
    feature_id: u32,
    contact_index: usize,
    manifold: &mut Convex4ContactManifoldWide,
) {
    let local_point = laned_info.laned_side_edge_center + laned_info.laned_cylinder_edge_axis * t;
    let contact_depth = (hull_face_origin - local_point).dot(laned_info.laned_hull_face_normal)
        * inverse_depth_denominator;
    let mut contact_offset_a = laned_info.laned_rb * local_point;
    contact_offset_a += laned_info.laned_offset_b;
    manifold.offset_a[contact_index].replace_lane(laned_info.laned_index, contact_offset_a);
    manifold.depth[contact_index].replace(laned_info.laned_index, contact_depth);
    manifold.feature_id[contact_index].replace(laned_info.laned_index, feature_id);
    manifold.contact_exists[contact_index].replace(laned_info.laned_index, true);
}

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

#[cfg(test)]
mod tests {
    use wasm_bindgen_test::*;

    use super::*;

    wasm_bindgen_test_configure!(run_in_browser);

    #[test]
    #[wasm_bindgen_test]
    pub fn test_intersect_line_circle_zero_line_direction() {
        let _ = env_logger::builder().is_test(true).try_init();

        let result = intersect_line_circle(Vec2::new(0.0, 0.0), Vec2::new(0.0, 1e-13), 1.0);
        assert_eq!(result, (true, 0.0, 0.0));
    }
}