phys-collision 2.0.1-beta.0

Provides collision detection ability
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

// 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::{BVec3x4, Mat3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4};

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

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

    // 1 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &SphereWide,
        b: &CuboidWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        let orientation_b = Mat3x4::from_quat(*contact_context.orientation_b);
        // From A to B.
        let local_offset_b = orientation_b.transpose() * contact_context.offset_b;
        let clamped_local_offset = local_offset_b.clamp(-b.half_length, b.half_length);
        // From B to A.
        let mut outside_normal = clamped_local_offset - local_offset_b;
        let distance = outside_normal.length();
        let inverse_distance = distance.recip();
        outside_normal *= inverse_distance;
        let outside_depth = a.radius - distance;

        // Find the shortest local axis, when the sphere center is inside the cuboid.
        let depth_x = b.half_length.x - local_offset_b.x.absf();
        let depth_y = b.half_length.y - local_offset_b.y.absf();
        let depth_z = b.half_length.z - local_offset_b.z.absf();
        let mut inside_depth = depth_x.min(depth_y).min(depth_z);
        let is_x_min = inside_depth.eq(depth_x);
        let is_y_min = inside_depth.eq(depth_y) & !is_x_min;
        let is_z_min = !(is_x_min | is_y_min);

        let mut inside_normal = Vec3x4::select(
            local_offset_b.cmplt(Vec3x4::ZERO),
            Vec3x4::ONE,
            Vec3x4::NEG_ONE,
        );
        inside_normal = Vec3x4::select(
            BVec3x4::new(is_x_min, is_y_min, is_z_min),
            inside_normal,
            Vec3x4::ZERO,
        );

        inside_depth += a.radius;
        let use_inside = distance.eq(f32x4::ZERO);
        let local_normal = Vec3x4::lane_select(use_inside, inside_normal, outside_normal);

        manifold.normal = (orientation_b * local_normal).as_unit_vec3x4_unchecked();
        manifold.depth[0] = inside_depth.select(use_inside, outside_depth);
        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, Cuboid, SphereWide, CuboidWide, 1);