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, UnitQuat, UnitQuatx4, UnitVec3x4, Vec3, Vec3x4};

use crate::collision_tasks::{ShapeTester, ShapeWideTester};
use crate::convex_contact_manifold::{Convex4ContactManifoldWide, ConvexContactManifold};
use crate::shapes::{Cylinder, CylinderWide, InfinitePlane, InfinitePlaneWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairTest, PairWideTest};
use crate::ShapeContainer;

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

    // 2 manifold in Convex4ContactManifoldWide
    fn test(
        a: &CylinderWide,
        _b: &InfinitePlaneWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        // convert cylinder's axis and top and bottom caps' centers into infinite plane space
        let cylinder_in_b = contact_context.orientation_b.inverse() * (-*contact_context.offset_b);
        let cylinder_axis_in_b =
            contact_context.orientation_b.inverse() * contact_context.orientation_a * UnitVec3x4::Y;
        let top_center_in_b = cylinder_in_b + cylinder_axis_in_b * a.half_height;
        let bottom_center_in_b = cylinder_in_b + cylinder_axis_in_b * (-a.half_height);

        // contact normal
        manifold.normal = contact_context.orientation_b * InfinitePlaneWide::NORMAL;

        // find the closest point on cylinder to the plane
        // the cylinder can be treated as a 2d box, we want to find the contact points from four
        // vertices.
        let contact_dir = ((UnitVec3x4::Y.cross(cylinder_axis_in_b)).cross(cylinder_axis_in_b))
            .normalize_or_zero();
        let top0 = top_center_in_b + contact_dir * a.radius;
        let top1 = top_center_in_b - contact_dir * a.radius;
        let bottom0 = bottom_center_in_b + contact_dir * a.radius;
        let bottom1 = bottom_center_in_b - contact_dir * a.radius;

        // if the cylinder is perpendicular to the plane, the contact_dir would equal to zero
        // vector, we use the bottom or top cap's center as contact point.
        let perpendicular = top0.distance_squared(top1).le(f32x4::EPSILON);
        let top0_contact = top0.y.le(contact_context.speculative_margin);
        let top1_contact = top1.y.le(contact_context.speculative_margin);
        let bottom0_contact = bottom0.y.le(contact_context.speculative_margin);
        let bottom1_contact = bottom1.y.le(contact_context.speculative_margin);
        let choose_top_center =
            perpendicular & top_center_in_b.y.le(contact_context.speculative_margin);
        let choose_bottom_center =
            perpendicular & bottom_center_in_b.y.le(contact_context.speculative_margin);
        let choose_top0 = top0_contact & top0.y.lt(top1.y);
        let choose_top1 = top1_contact & top1.y.lt(top0.y);
        let choose_bottom0 = bottom0_contact & bottom0.y.lt(bottom1.y);
        let choose_bottom1 = bottom1_contact & bottom1.y.lt(bottom0.y);

        let contact0 = Vec3x4::lane_select(
            choose_top_center,
            top_center_in_b,
            Vec3x4::lane_select(
                choose_top0,
                top0,
                Vec3x4::lane_select(choose_top1, top1, Vec3x4::default()),
            ),
        );
        let contact1 = Vec3x4::lane_select(
            choose_bottom_center,
            bottom_center_in_b,
            Vec3x4::lane_select(
                choose_bottom0,
                bottom0,
                Vec3x4::lane_select(choose_bottom1, bottom1, Vec3x4::default()),
            ),
        );

        manifold.depth[0] = (-top_center_in_b.y).select(
            choose_top_center,
            (-top0.y).select(
                choose_top0,
                (-top1.y).select(choose_top1, f32x4::const_splat(f32::MIN)),
            ),
        );
        manifold.depth[1] = (-bottom_center_in_b.y).select(
            choose_bottom_center,
            (-bottom0.y).select(
                choose_bottom0,
                (-bottom1.y).select(choose_bottom1, f32x4::const_splat(f32::MIN)),
            ),
        );

        manifold.contact_exists[0] = manifold.depth[0].ge(-contact_context.speculative_margin);
        manifold.contact_exists[1] = manifold.depth[1].ge(-contact_context.speculative_margin);

        // the contacts are on cylinder's surface in plane's space, subtract with cylinder's center
        // to get the offset and transform back into world spcace
        manifold.offset_a[0] = contact_context.orientation_b * (contact0 - cylinder_in_b);
        manifold.offset_a[1] = contact_context.orientation_b * (contact1 - cylinder_in_b);

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

impl_pair_narrowphase!(Cylinder, InfinitePlane, CylinderWide, InfinitePlaneWide, 2);