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::num_traits::*;
use glam_det::nums::{f32x4, u32x4};
use glam_det::{UnitQuat, UnitQuatx4, UnitVec3x4, Vec3, Vec3x4};

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

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

    // 1 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &SphereWide,
        b: &SphereWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        let center_distance = contact_context.offset_b.length();
        let inverse_distance = -center_distance.recip();
        manifold.normal = (contact_context.offset_b * inverse_distance).as_unit_vec3x4_unchecked();
        let normal_is_valid = center_distance.gt(f32x4::ZERO);

        manifold.normal = UnitVec3x4::lane_select(normal_is_valid, manifold.normal, UnitVec3x4::Y);

        manifold.depth[0] = a.radius + b.radius - center_distance;

        let negative_offset_from_a = -a.radius;
        manifold.offset_a[0] = manifold.normal * negative_offset_from_a;
        manifold.contact_exists[0] = manifold.depth[0].gt(-contact_context.speculative_margin);
        manifold.feature_id[0] = u32x4::ZERO;
    }
}

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