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 core::ops::Not;

use glam_det::nums::{f32x4, u32x4, Bool, Float, Num, NumConstEx, PartialOrdEx};
use glam_det::{
    Cross, Dot, Mat3x4, Point3, UnitQuat, UnitQuatx4, UnitVec3, UnitVec3x4, Vec3, Vec3x4,
};

use crate::collision_tasks::common::{NormalizeExt, EPS_3, EPS_5, EPS_8};
use crate::collision_tasks::traits::TransformWide;
use crate::collision_tasks::{tootbird, ShapeWideTester};
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::{BundleIndex, CapsuleWide, ConvexHullWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairWideTest};
use crate::{
    Capsule, ConvexContactManifold, ConvexHull, ConvexHullId, PairTest, ShapeContainer, ShapeTester,
};

impl PairWideTest<CapsuleWide, ConvexHullWide> for ShapeWideTester {
    // 2 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &CapsuleWide,
        b: &ConvexHullWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        const MAXIMUM_ITERRATIONS: usize = 25;

        // Following compute are all in the b local space

        let container = contact_context
            .complex_shape_container
            .expect("container must be Some");
        let rb = Mat3x4::from_quat(*contact_context.orientation_b);
        // rb_inverse is used to transform from world space to b local space
        let rb_inverse = rb.transpose();
        let ra = rb_inverse * Mat3x4::from_quat(*contact_context.orientation_a);
        let ra_y_axis = &ra.y_axis;
        let shift_b = b.get_convexhull_shift_wide(container, contact_context.pair_count);
        let center_b = b.get_center(container, contact_context.pair_count);
        let local_offset_a = -(rb_inverse * contact_context.offset_b) - shift_b;
        let normal_b_to_a = (local_offset_a + center_b).normalize_or(Vec3x4::Y, EPS_8);

        let mut inactive_lanes =
            u32x4::splat(contact_context.pair_count as u32).le(u32x4::from([0, 1, 2, 3]));
        let hull_epsilon_scale = b.estimate_epsilon_scale(inactive_lanes, container);
        let epsilon_scale = a.radius.min(hull_epsilon_scale);
        let minimum_accepted_depth = -contact_context.speculative_margin;
        // issue (#1349) change tootbird from Matrix to Quat
        let toot_bird_result = tootbird::find_minimum_depth(
            b,
            a,
            &TransformWide::new(local_offset_a, &ra),
            normal_b_to_a,
            &tootbird::IterContext::new(
                inactive_lanes,
                epsilon_scale * EPS_5,
                minimum_accepted_depth,
                MAXIMUM_ITERRATIONS,
                contact_context.complex_shape_container,
                true,
            ),
        );
        inactive_lanes = inactive_lanes | toot_bird_result.depth.lt(minimum_accepted_depth);
        if inactive_lanes.all() {
            manifold.reset(2);
            return;
        }

        let bounding_plane_epsilon = EPS_3 * epsilon_scale;
        let mut face_normal_x4 = UnitVec3x4::X;
        let mut min_capsule_to_edge_x4 = f32x4::ZERO;
        let mut min_direction_a_dot_edge_plane_normal_x4 = f32x4::ZERO;
        let mut max_capsule_to_edge_x4 = f32x4::ZERO;
        let mut max_direction_a_dot_edge_plane_normal_x4 = f32x4::ZERO;
        for (lane_index, inactive) in <[bool; 4]>::from(inactive_lanes)
            .into_iter()
            .enumerate()
            .take(contact_context.pair_count)
        {
            if inactive {
                continue;
            }
            let hull_id = b.get_id(lane_index);
            let hull = container
                .get::<ConvexHull>(hull_id)
                .expect("hull must be Some");
            let face = hull.pick_representative_face(
                toot_bird_result.normal.extract_lane(lane_index),
                Point3::from_vec3(toot_bird_result.closest_point_on_a.extract_lane(lane_index)),
                bounding_plane_epsilon.extract(lane_index),
            );
            let face_vertex_indices = hull.get_vertex_indices(face.face_index);
            face_normal_x4.replace_lane(lane_index, face.normal);

            // Test the capsule segment and face edges.
            let (
                min_capsule_to_edge,
                min_direction_a_dot_edge_plane_normal,
                max_capsule_to_edge,
                max_direction_a_dot_edge_plane_normal,
            ) = select_range_on_segment_vs_hull_face(
                ra.y_axis.extract_lane(lane_index),
                local_offset_a.extract_lane(lane_index),
                a.half_height.extract(lane_index),
                toot_bird_result.normal.extract_lane(lane_index),
                hull,
                face_vertex_indices,
            );
            min_capsule_to_edge_x4.replace(lane_index, min_capsule_to_edge);
            min_direction_a_dot_edge_plane_normal_x4
                .replace(lane_index, min_direction_a_dot_edge_plane_normal);
            max_capsule_to_edge_x4.replace(lane_index, max_capsule_to_edge);
            max_direction_a_dot_edge_plane_normal_x4
                .replace(lane_index, max_direction_a_dot_edge_plane_normal);
        }

        let mut t_min = min_capsule_to_edge_x4 / min_direction_a_dot_edge_plane_normal_x4;
        let mut t_max = max_capsule_to_edge_x4 / max_direction_a_dot_edge_plane_normal_x4;
        let negative_half_length = -a.half_height;
        t_min = t_min.clamp(negative_half_length, a.half_height);
        t_max = t_max.clamp(negative_half_length, a.half_height);

        // offset to capsule center in hull local space
        let local_offset_0 = ra_y_axis * t_min;
        let local_offset_1 = ra_y_axis * t_max;
        // toot_bird_result.closest_point_on_a is closest_point_on_b for us
        let a_to_point_on_hull_face = toot_bird_result.closest_point_on_a - local_offset_a;
        // The scale of product between face_normal and hull closest point normal.
        let scale = face_normal_x4.dot(toot_bird_result.normal.as_vec3x4());
        // Using scale_recip to unproject the depth to closest point normal.
        let scale_recip = scale.recip();
        let contact0_to_hull_face = a_to_point_on_hull_face - local_offset_0;
        let contact1_to_hull_face = a_to_point_on_hull_face - local_offset_1;
        let depth_on_face_normal_0 = face_normal_x4.dot(contact0_to_hull_face);
        let depth_on_face_normal_1 = face_normal_x4.dot(contact1_to_hull_face);
        let depth_on_closest_normal_0 = depth_on_face_normal_0 * scale_recip;
        let depth_on_closest_normal_1 = depth_on_face_normal_1 * scale_recip;
        manifold.depth[0] = a.radius + depth_on_closest_normal_0;
        manifold.depth[1] = a.radius + depth_on_closest_normal_1;
        manifold.feature_id[0] = u32x4::splat(0);
        manifold.feature_id[1] = u32x4::splat(1);
        manifold.contact_exists[0] =
            manifold.depth[0].ge(minimum_accepted_depth) & (inactive_lanes.not());
        manifold.contact_exists[1] = (t_max - t_min).gt(a.half_height * EPS_3)
            & manifold.depth[1].ge(minimum_accepted_depth)
            & (inactive_lanes.not());
        manifold.offset_a[0] = rb * local_offset_0;
        manifold.offset_a[1] = rb * local_offset_1;
        manifold.normal = (rb * toot_bird_result.normal).as_unit_vec3x4_unchecked();
        let contact_offset_0 = manifold.normal * depth_on_closest_normal_0;
        let contact_offset_1 = manifold.normal * depth_on_closest_normal_1;
        manifold.offset_a[0] += contact_offset_0;
        manifold.offset_a[1] += contact_offset_1;
        manifold.offset_a[0] -= manifold.normal * manifold.depth[0];
        manifold.offset_a[1] -= manifold.normal * manifold.depth[1];
    }

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

/// Test the segment and face edges.
/// Return `(min_segment_to_edge, min_direction_a_dot_edge_plane_normal, max_segment_to_edge,
/// max_direction_a_dot_edge_plane_normal)`.
/// It's the range on the segment that may contact with hull edge.
/// Range: `[min_segment_to_edge/min_direction_a_dot_edge_plane_normal,
/// max_segment_to_edge/max_direction_a_dot_edge_plane_normal]`.
/// We didn't divide here, because we can do it later by wide, and also clamp for segment then.
pub(super) fn select_range_on_segment_vs_hull_face(
    direction_a: Vec3,
    local_offset_a: Vec3,
    a_half_length: f32,
    contact_normal: UnitVec3,
    hull: &ConvexHull,
    face_vertex_indices: &[BundleIndex],
) -> (f32, f32, f32, f32) {
    const MIN: f32 = 1e-5;
    const MAX: f32 = 3e-4;
    const INVERSE_SPAN: f32 = 1.0f32 / (MAX - MIN);

    let last_index = face_vertex_indices[face_vertex_indices.len() - 1];
    let mut vertex = hull.get_point(last_index);
    let mut min_segment_to_edge = f32::MAX;
    let mut min_direction_a_dot_edge_plane_normal = -1.0;
    let mut max_segment_to_edge = f32::MAX;
    let mut max_direction_a_dot_edge_plane_normal = 1.0;

    // Now we have the vertices of the face, consider the segment as a line first,
    // and test intersection of line and edge_plane(edge and contact_normal).
    //  * Project a_to_vertex to edge_plane_normal
    //  * Unproject it back to segment, we get intersection.
    //  * Keep the max and min intersection of all edge planes.

    // Iterate edges of vertices: (n,0),(0,1)...(n-1,n)
    for &current_index in face_vertex_indices {
        let neighbour = hull.get_point(current_index);
        let edge_offset = neighbour - vertex;
        let edge_plane_normal = edge_offset.cross(contact_normal);

        let a_to_vertex = vertex.as_vec3() - local_offset_a;
        let mut segment_to_edge_on_plane_normal = a_to_vertex.dot(edge_plane_normal);
        // Scale of direction_a project to edge_plane_normal.
        // Later we can get intersection of segment and edge_plane by
        // segment_to_edge_on_plane_normal/direction_a_dot_edge_plane_normal
        let direction_a_dot_edge_plane_normal = direction_a.dot(edge_plane_normal);
        vertex = neighbour;

        let edge_length_squared = edge_offset.length_squared();
        let scale_squared = direction_a_dot_edge_plane_normal * direction_a_dot_edge_plane_normal;

        // If scale_squared is zero, it means the segment line is perpendicular to the
        // edge_plane_normal, the line is parallel to the edge, we can skip this edge.
        if scale_squared > MIN * edge_length_squared {
            // If the segment is almost parallel to the edge, we fix the intersection close
            // to segment endpoint.
            if scale_squared < MAX * edge_length_squared {
                let mut restrict_weight =
                    (scale_squared / edge_length_squared - MIN) * INVERSE_SPAN;
                restrict_weight = restrict_weight.clamp(0.0, 1.0);
                let mut unrestricted_numerator = a_half_length * direction_a_dot_edge_plane_normal;
                if direction_a_dot_edge_plane_normal < 0f32 {
                    unrestricted_numerator = -unrestricted_numerator;
                }
                segment_to_edge_on_plane_normal = restrict_weight * segment_to_edge_on_plane_normal
                    + (1.0 - restrict_weight) * unrestricted_numerator;
            }

            // Compare segment_to_edge_on_plane_normal/direction_a_dot_edge_plane_normal
            // which is the intersection of segment and edge_plane. Keep the max and
            // min.
            if direction_a_dot_edge_plane_normal < 0f32 {
                if segment_to_edge_on_plane_normal * min_direction_a_dot_edge_plane_normal
                    > min_segment_to_edge * direction_a_dot_edge_plane_normal
                {
                    min_segment_to_edge = segment_to_edge_on_plane_normal;
                    min_direction_a_dot_edge_plane_normal = direction_a_dot_edge_plane_normal;
                }
            } else if segment_to_edge_on_plane_normal * max_direction_a_dot_edge_plane_normal
                < max_segment_to_edge * direction_a_dot_edge_plane_normal
            {
                max_segment_to_edge = segment_to_edge_on_plane_normal;
                max_direction_a_dot_edge_plane_normal = direction_a_dot_edge_plane_normal;
            }
        }
    }
    (
        min_segment_to_edge,
        min_direction_a_dot_edge_plane_normal,
        max_segment_to_edge,
        max_direction_a_dot_edge_plane_normal,
    )
}

#[cfg(test)]
mod tests {
    use approx_det::assert_relative_eq;
    use glam_det::nums::{f32x4, Num};
    use glam_det::{Isometry3, UnitQuat, UnitQuatx4, Vec3, Vec3x4};
    use wasm_bindgen_test::wasm_bindgen_test;

    use crate::collision_tasks::ShapeWideTester;
    use crate::convex_contact_manifold::Convex4ContactManifoldWide;
    use crate::shapes::{CapsuleWide, ConvexHullWide, CuboidExt};
    use crate::traits::{ContactContext, CreateShapeWide, PairWideTest};
    use crate::{Capsule, ConvexHull, Cuboid, ShapeContainer};

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

        let a = Capsule::new(0.5, 0.5);
        let b = Cuboid::new(Vec3::splat(2.0));
        let points_b = (0..8).map(|i| b.get_vertex(i)).collect::<Vec<_>>();
        let b = ConvexHull::new_unchecked(&points_b);
        let mut container = ShapeContainer::default();
        let b = container.add(b);
        let transform_a = Isometry3::IDENTITY;
        let transform_b = Isometry3::from_translation(Vec3::new(0.0, 0.0, 1.4));
        let a_wide = <CapsuleWide as CreateShapeWide<1>>::create([&a].into_iter());
        let b_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&b].into_iter());
        let speculative_margin = f32x4::splat(0.01);
        let offset_b = Vec3x4::splat_soa(transform_b.translation - transform_a.translation);
        let orientation_a = UnitQuatx4::splat_soa(transform_a.rotation);
        let orientation_b = UnitQuatx4::splat_soa(transform_b.rotation);
        let mut manifold = Convex4ContactManifoldWide::default();
        let context = ContactContext {
            orientation_a: &orientation_a,
            orientation_b: &orientation_b,
            offset_b: &offset_b,
            speculative_margin,
            complex_shape_container: Some(&container),
            pair_count: 1,
        };
        ShapeWideTester::test(&a_wide, &b_wide, &context, &mut manifold);
        assert_relative_eq!(manifold.offset_a[0].extract_lane(0).z, 0.5f32);
        assert_relative_eq!(manifold.offset_a[1].extract_lane(0).z, 0.5f32);
    }

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

        let a = Capsule::new(0.5, 0.5);
        let b = Cuboid::new(Vec3::splat(2.0));
        let points_b = (0..8).map(|i| b.get_vertex(i)).collect::<Vec<_>>();
        let b = ConvexHull::new_unchecked(&points_b);
        let mut container = ShapeContainer::default();
        let b = container.add(b);
        let transform_a = Isometry3::from_quat(UnitQuat::from_rotation_x(90f32.to_radians()));
        let transform_b = Isometry3::from_translation(Vec3::new(0.0, 0.0, 1.9));
        let a_wide = <CapsuleWide as CreateShapeWide<1>>::create([&a].into_iter());
        let b_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&b].into_iter());
        let speculative_margin = f32x4::splat(0.01);
        let offset_b = Vec3x4::splat_soa(transform_b.translation - transform_a.translation);
        let orientation_a = UnitQuatx4::splat_soa(transform_a.rotation);
        let orientation_b = UnitQuatx4::splat_soa(transform_b.rotation);
        let mut manifold = Convex4ContactManifoldWide::default();
        let context = ContactContext {
            orientation_a: &orientation_a,
            orientation_b: &orientation_b,
            offset_b: &offset_b,
            speculative_margin,
            complex_shape_container: Some(&container),
            pair_count: 1,
        };
        ShapeWideTester::test(&a_wide, &b_wide, &context, &mut manifold);
        assert_relative_eq!(manifold.offset_a[0].extract_lane(0).z, 0.0f32);
        assert_relative_eq!(manifold.offset_a[1].extract_lane(0).z, 1.0f32);
    }
}

impl_pair_narrowphase!(Capsule, ConvexHullId, CapsuleWide, ConvexHullWide, 2);