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 std::ops::Neg;

use glam_det::nums::num_traits::*;
use glam_det::nums::{f32x4, u32x4};
use glam_det::{Dot, Mat3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4};

use super::common::NormalizeExt;
use crate::collision_tasks::{ShapeTester, ShapeWideTester};
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::{Capsule, CapsuleWide, Sphere, SphereWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairTest, PairWideTest};
use crate::{ConvexContactManifold, ShapeContainer};

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

    // 1 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &SphereWide,
        b: &CapsuleWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        // Find the point on the capsule's internal line segment, which is closest to the sphere
        // center.
        let Mat3x4 { x_axis, y_axis, .. } = Mat3x4::from_quat(*contact_context.orientation_b);
        let point = y_axis
            .dot(contact_context.offset_b) // product offset_b to rotated B local y axis 
            .neg() // offset_a product on B local y axis
            .clamp(-b.half_height, b.half_height) // clamp to the endpoints of line segment
            * y_axis // capsule local
            + contact_context.offset_b; // sphere local

        let (internal_length, normal) = point.normalize_and_length_or(x_axis, f32x4::ZERO);
        // B to A
        manifold.normal = normal.neg().as_unit_vec3x4_unchecked();
        manifold.depth[0] = a.radius + b.radius - internal_length;
        manifold.feature_id[0] = u32x4::ZERO;
        manifold.offset_a[0] = manifold.normal * (-a.radius);
        manifold.contact_exists[0] = manifold.depth[0].gt(-contact_context.speculative_margin);
    }
}

impl_pair_narrowphase!(Sphere, Capsule, SphereWide, CapsuleWide, 1);