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
106
107
108
109
110
111
112
113
114
115
116
117

// 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::common_types::ConstUnionHack128bit;
use glam_det::nums::*;
use glam_det::{Dot, Point3, Point3x4, Vec3x4};
// Re-export Triangle from geom for backward compatibility
pub use phys_geom::shape::Triangle;

use crate::{
    ArrayGetter, BaseShapeWide, ConvexShape, CreateShapeWide, MinkowskiSupportResultWide,
    MinkowskiSupportWide, ShapeContainer,
};

impl ConvexShape for Triangle {}

#[derive(Debug)]
pub struct TriangleWide {
    pub a: Point3x4,
    pub b: Point3x4,
    pub c: Point3x4,
}

impl Default for TriangleWide {
    #[inline]
    fn default() -> Self {
        Self {
            a: Point3x4::X,
            b: Point3x4::Y,
            c: Point3x4::Z,
        }
    }
}

impl BaseShapeWide for TriangleWide {
    type TShape = Triangle;
}

impl MinkowskiSupportWide for TriangleWide {
    fn support_point_local(
        &self,
        local_direction: Vec3x4,
        _container: Option<&ShapeContainer>,
    ) -> MinkowskiSupportResultWide {
        let vert_a = self.a.as_vec3x4();
        let vert_b = self.b.as_vec3x4();
        let vert_c = self.c.as_vec3x4();
        let dot_a = Vec3x4::dot(vert_a, local_direction);
        let dot_b = Vec3x4::dot(vert_b, local_direction);
        let dot_c = Vec3x4::dot(vert_c, local_direction);
        let max = f32x4::maxf(dot_a, f32x4::maxf(dot_b, dot_c));
        let support = Point3x4::lane_select(
            max.eq(dot_a),
            self.a,
            Point3x4::lane_select(max.eq(dot_b), self.b, self.c),
        );
        MinkowskiSupportResultWide {
            point: support,
            point_index: u32x4::ZERO,
        }
    }
}

impl TriangleWide {
    pub const BACKFACE_THRESHOLD: f32x4 =
        unsafe { ConstUnionHack128bit { f32a4: [-1e-2; 4] }.f32x4 };
    const DEGENERATE_TRIANGLE_EPSILON: f32x4 =
        unsafe { ConstUnionHack128bit { f32a4: [1e-6; 4] }.f32x4 };

    #[allow(dead_code)]
    #[inline]
    #[must_use]
    pub fn compute_non_degenerate_triangle_mask(
        edge0: &Vec3x4,
        edge1: &Vec3x4,
        normal_length: &f32x4,
    ) -> bool32x4 {
        let epsilon_scale = edge0.length_squared().max(edge1.length_squared()).sqrtf();
        normal_length.gt(epsilon_scale * TriangleWide::DEGENERATE_TRIANGLE_EPSILON)
    }
}

use glamdet_na_conv::ConvTo;
macro_rules! impl_triangle_wide {
    ($($num:tt),*) => {
            $(
                impl CreateShapeWide<$num> for TriangleWide {
                    fn create<'a>(iter: impl Iterator<Item=&'a Self::TShape> + Clone) -> Self where Self::TShape: 'a {
                        Self {
                            a: ArrayGetter::<$num>::get_point3x4_from_array4(ArrayGetter::<$num>::get_array4_from_iter(iter.clone().map(#[inline]|shape| {
                                shape.a.conv_to()
                            }), Point3::X)),
                            b: ArrayGetter::<$num>::get_point3x4_from_array4(ArrayGetter::<$num>::get_array4_from_iter(iter.clone().map(#[inline]|shape| {
                                shape.b.conv_to()
                            }), Point3::Y)),
                            c: ArrayGetter::<$num>::get_point3x4_from_array4(ArrayGetter::<$num>::get_array4_from_iter(iter.clone().map(#[inline]|shape| {
                                shape.c.conv_to()
                            }), Point3::Z)),
                        }
                    }
                }
            )*
    };
}

impl_triangle_wide!(1, 2, 3, 4);