parry2d/shape/
triangle_pseudo_normals.rs

1use crate::math::Vector;
2
3#[cfg(feature = "alloc")]
4use crate::{math::Vector3, query::details::NormalConstraints};
5
6// NOTE: ideally, the normal cone should take into account the point where the normal cone is
7//       considered. But as long as we assume that the triangles are one-way we can get away with
8//       just relying on the normal directions.
9//       Taking the point into account would be technically doable (and desirable if we wanted
10//       to define, e.g., a one-way mesh) but requires:
11//       1. To make sure the edge pseudo-normals are given in the correct edge order.
12//       2. To have access to the contact feature.
13//       We can have access to both during the narrow-phase, but leave that as a future
14//       potential improvements.
15// NOTE: this isn’t equal to the "true" normal cones since concave edges will have pseudo-normals
16//       still pointing outward (instead of inward or being empty).
17/// The pseudo-normals of a triangle providing approximations of its feature’s normal cones.
18#[derive(Clone, Debug)]
19pub struct TrianglePseudoNormals {
20    /// The triangle’s face normal.
21    pub face: Vector,
22    // TODO: if we switch this to providing pseudo-normals in a specific order
23    //       (e.g. in the same order as the triangle’s edges), then we should
24    //       think of fixing that order in the heightfield
25    //       triangle_pseudo_normals code.
26    /// The edges pseudo-normals, in no particular order.
27    pub edges: [Vector; 3],
28}
29
30#[cfg(feature = "alloc")]
31impl NormalConstraints for TrianglePseudoNormals {
32    /// Projects the given direction to it is contained in the polygonal
33    /// cone defined `self`.
34    fn project_local_normal_mut(&self, dir: &mut Vector) -> bool {
35        // Find the closest pseudo-normal.
36        let dots = Vector3::new(
37            dir.dot(self.edges[0]),
38            dir.dot(self.edges[1]),
39            dir.dot(self.edges[2]),
40        );
41        let closest_edge = self.edges[dots.max_position()];
42        crate::shape::pseudo_normals::project_into_cone(self.face, closest_edge, dir)
43    }
44}
45
46#[cfg(test)]
47#[cfg(all(feature = "dim3", feature = "alloc"))]
48mod test {
49    use super::NormalConstraints;
50    use crate::math::{Real, Vector};
51    use crate::shape::TrianglePseudoNormals;
52
53    fn bisector(v1: Vector, v2: Vector) -> Vector {
54        (v1 + v2).normalize()
55    }
56
57    fn bisector_y(v: Vector) -> Vector {
58        bisector(v, Vector::Y)
59    }
60
61    #[test]
62    fn trivial_pseudo_normals_projection() {
63        let pn = TrianglePseudoNormals {
64            face: Vector::Y,
65            edges: [Vector::Y; 3],
66        };
67
68        assert_eq!(
69            pn.project_local_normal(Vector::new(1.0, 1.0, 1.0)),
70            Some(Vector::Y)
71        );
72        assert!(pn.project_local_normal(-Vector::Y).is_none());
73    }
74
75    #[test]
76    fn edge_pseudo_normals_projection_strictly_positive() {
77        let bisector = |v1: Vector, v2: Vector| (v1 + v2).normalize();
78        let bisector_y = |v: Vector| bisector(v, Vector::Y);
79
80        // The normal cones for this test will be fully contained in the +Y half-space.
81        let cones_ref_dir = [
82            -Vector::Z,
83            -Vector::X,
84            Vector::new(1.0, 0.0, 1.0).normalize(),
85        ];
86        let cones_ends = cones_ref_dir.map(bisector_y);
87        let cones_axes = cones_ends.map(bisector_y);
88
89        let pn = TrianglePseudoNormals {
90            face: Vector::Y,
91            edges: cones_axes.map(|v| v.normalize()),
92        };
93
94        for i in 0..3 {
95            assert!(pn
96                .project_local_normal(cones_ends[i])
97                .unwrap()
98                .abs_diff_eq(cones_ends[i], 1.0e-5));
99            assert_eq!(pn.project_local_normal(cones_axes[i]), Some(cones_axes[i]));
100
101            // Guaranteed to be inside the normal cone of edge i.
102            let subdivs = 100;
103
104            for k in 1..100 {
105                let v = Vector::Y
106                    .lerp(cones_ends[i], k as Real / (subdivs as Real))
107                    .normalize();
108                assert_eq!(pn.project_local_normal(v).unwrap(), v);
109            }
110
111            // Guaranteed to be outside the normal cone of edge i.
112            for k in 1..subdivs {
113                let v = cones_ref_dir[i]
114                    .lerp(cones_ends[i], k as Real / (subdivs as Real))
115                    .normalize();
116                assert!(pn
117                    .project_local_normal(v)
118                    .unwrap()
119                    .abs_diff_eq(cones_ends[i], 1.0e-5));
120            }
121
122            // Guaranteed to be outside the normal cone, and in the -Y half-space.
123            for k in 1..subdivs {
124                let v = cones_ref_dir[i]
125                    .lerp(-Vector::Y, k as Real / (subdivs as Real))
126                    .normalize();
127                assert!(pn.project_local_normal(v).is_none(),);
128            }
129        }
130    }
131
132    #[test]
133    fn edge_pseudo_normals_projection_negative() {
134        // The normal cones for this test will be fully contained in the +Y half-space.
135        let cones_ref_dir = [
136            -Vector::Z,
137            -Vector::X,
138            Vector::new(1.0, 0.0, 1.0).normalize(),
139        ];
140        let cones_ends = cones_ref_dir.map(|v| bisector(v, -Vector::Y));
141        let cones_axes = [
142            bisector(bisector_y(cones_ref_dir[0]), cones_ref_dir[0]),
143            bisector(bisector_y(cones_ref_dir[1]), cones_ref_dir[1]),
144            bisector(bisector_y(cones_ref_dir[2]), cones_ref_dir[2]),
145        ];
146
147        let pn = TrianglePseudoNormals {
148            face: Vector::Y,
149            edges: cones_axes.map(|v| v.normalize()),
150        };
151
152        for i in 0..3 {
153            assert_eq!(pn.project_local_normal(cones_axes[i]), Some(cones_axes[i]));
154
155            // Guaranteed to be inside the normal cone of edge i.
156            let subdivs = 100;
157
158            for k in 1..subdivs {
159                let v = Vector::Y
160                    .lerp(cones_ends[i], k as Real / (subdivs as Real))
161                    .normalize();
162                assert_eq!(pn.project_local_normal(v).unwrap(), v);
163            }
164
165            // Guaranteed to be outside the normal cone of edge i.
166            // Since it is additionally guaranteed to be in the -Y half-space, we should get None.
167            for k in 1..subdivs {
168                let v = (-Vector::Y)
169                    .lerp(cones_ends[i], k as Real / (subdivs as Real))
170                    .normalize();
171                assert!(pn.project_local_normal(v).is_none());
172            }
173        }
174    }
175}
parry2d/shape/triangle_pseudo_normals.rs

parry2d/shape/
triangle_pseudo_normals.rs
