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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105

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

use crate::{
    ContactContextTester as ContactContext, ContactManifoldWide, Convex4ContactManifoldWide,
    ConvexContactManifold, CreateShapeWide, PairTest, PairWideTest, ShapeContainer, ShapeTester,
    ShapeWideTester, Sphere, SphereWide, Triangle, TriangleWide,
};

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

    // 1 manifold in Convex4ContactManifoldWide
    #[inline]
    fn test(
        a: &SphereWide,
        local_triangle: &TriangleWide,
        contact_context: &ContactContext,
        manifold: &mut Convex4ContactManifoldWide,
    ) {
        let rotation_b = Mat3x4::from_quat(*contact_context.orientation_b);
        let local_offset_b = rotation_b.transpose() * *contact_context.offset_b;
        let speculative_margin = contact_context.speculative_margin;

        let ab = local_triangle.b - local_triangle.a;
        let ac = local_triangle.c - local_triangle.a;
        //Work in triangle's local space.
        //P is the center of the sphere. where op + pa = oa, pa = oa - op (op is `-local_offset_b`)
        let pa = (local_triangle.a + local_offset_b).as_vec3x4();

        //attention: this normal is backface normal.
        let mut local_triangle_normal = Vec3x4::cross(ab, ac);
        let triangle_normal_length = local_triangle_normal.length();
        let inverse_triangle_normal_length = triangle_normal_length.recip();
        local_triangle_normal *= inverse_triangle_normal_length;

        let pa_x_ab = Vec3x4::cross(pa, ab);
        let ac_x_pa = Vec3x4::cross(ac, pa);

        //check if sphere center is in triangle in the projection plane alone the normal.
        let edge_plane_test_ab = Vec3x4::dot(pa_x_ab, local_triangle_normal);
        let edge_plane_test_ac = Vec3x4::dot(ac_x_pa, local_triangle_normal);
        let edge_plane_test_bc =
            f32x4::ONE - (edge_plane_test_ab + edge_plane_test_ac) * inverse_triangle_normal_length;

        let outside_ab = edge_plane_test_ab.lt(f32x4::ZERO);
        let outside_bc = edge_plane_test_bc.lt(f32x4::ZERO);
        let outside_ac = edge_plane_test_ac.lt(f32x4::ZERO);

        let outside_any_edge = outside_ab | outside_bc | outside_ac;

        let active_lanes =
            !(u32x4::splat(contact_context.pair_count as u32).le(u32x4::from([0, 1, 2, 3])));

        //classic formular - intersection of ray and plane.
        let pa_n = Vec3x4::dot(local_triangle_normal, pa);
        let offset_to_plane = pa_n * local_triangle_normal;
        let point_on_face = offset_to_plane - local_offset_b;

        let local_closest_on_triangle = point_on_face;

        manifold.feature_id[0] = u32x4::select(u32x4::ZERO, outside_any_edge, u32x4::ONE);

        let offset_a = (rotation_b * local_closest_on_triangle) + *contact_context.offset_b;
        let distance = offset_a.length();
        let normal_scale = -distance.recip();
        let world_normal = (offset_a * normal_scale).as_unit_vec3x4_unchecked();
        let face_normal_dot_local_normal =
            Vec3x4::dot(rotation_b * local_triangle_normal, world_normal);

        //backface process.
        let backface_normal = -world_normal;
        let backface_depth = distance + a.radius;
        let is_backface = face_normal_dot_local_normal.gt(f32x4::ZERO);

        manifold.normal = UnitVec3x4::lane_select(is_backface, backface_normal, world_normal);
        manifold.depth[0] = f32x4::select(backface_depth, is_backface, a.radius - distance);

        manifold.offset_a[0] = offset_a;
        manifold.contact_exists[0] = !outside_any_edge
            & active_lanes
            & distance.gt(f32x4::ZERO)
            & manifold.depth[0].ge(-speculative_margin);
    }
}

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