parry3d/shape/
triangle_pseudo_normals.rs

1#[cfg(feature = "alloc")]
2use crate::math::Vector;
3use crate::math::{Real, UnitVector};
4#[cfg(feature = "alloc")]
5use na::Vector3;
6
7#[cfg(feature = "alloc")]
8use crate::query::details::NormalConstraints;
9
10// NOTE: ideally, the normal cone should take into account the point where the normal cone is
11//       considered. But as long as we assume that the triangles are one-way we can get away with
12//       just relying on the normal directions.
13//       Taking the point into account would be technically doable (and desirable if we wanted
14//       to define, e.g., a one-way mesh) but requires:
15//       1. To make sure the edge pseudo-normals are given in the correct edge order.
16//       2. To have access to the contact feature.
17//       We can have access to both during the narrow-phase, but leave that as a future
18//       potential improvements.
19// NOTE: this isn’t equal to the "true" normal cones since concave edges will have pseudo-normals
20//       still pointing outward (instead of inward or being empty).
21/// The pseudo-normals of a triangle providing approximations of its feature’s normal cones.
22#[derive(Clone, Debug)]
23pub struct TrianglePseudoNormals {
24    /// The triangle’s face normal.
25    pub face: UnitVector<Real>,
26    // TODO: if we switch this to providing pseudo-normals in a specific order
27    //       (e.g. in the same order as the triangle’s edges), then we should
28    //       think of fixing that order in the heightfield
29    //       triangle_pseudo_normals code.
30    /// The edges pseudo-normals, in no particular order.
31    pub edges: [UnitVector<Real>; 3],
32}
33
34#[cfg(feature = "alloc")]
35impl NormalConstraints for TrianglePseudoNormals {
36    /// Projects the given direction to it is contained in the polygonal
37    /// cone defined `self`.
38    fn project_local_normal_mut(&self, dir: &mut Vector<Real>) -> bool {
39        let dot_face = dir.dot(&self.face);
40
41        // Find the closest pseudo-normal.
42        let dots = Vector3::new(
43            dir.dot(&self.edges[0]),
44            dir.dot(&self.edges[1]),
45            dir.dot(&self.edges[2]),
46        );
47        let closest_dot = dots.imax();
48        let closest_edge = &self.edges[closest_dot];
49
50        // Apply the projection. Note that this isn’t 100% accurate since this approximates the
51        // vertex normal cone using the closest edge’s normal cone instead of the
52        // true polygonal cone on S² (but taking into account this polygonal cone exactly
53        // would be quite computationally expensive).
54
55        if *closest_edge == self.face {
56            // The normal cone is degenerate, there is only one possible direction.
57            *dir = *self.face;
58            return dot_face >= 0.0;
59        }
60
61        // TODO: take into account the two closest edges instead for better continuity
62        //       of vertex normals?
63        let dot_edge_face = self.face.dot(closest_edge);
64        let dot_dir_face = self.face.dot(dir);
65        let dot_corrected_dir_face = 2.0 * dot_edge_face * dot_edge_face - 1.0; // cos(2 * angle(closest_edge, face))
66
67        if dot_dir_face >= dot_corrected_dir_face {
68            // The direction is in the pseudo-normal cone. No correction to apply.
69            return true;
70        }
71
72        // We need to correct.
73        let edge_on_normal = *self.face * dot_edge_face;
74        let edge_orthogonal_to_normal = **closest_edge - edge_on_normal;
75
76        let dir_on_normal = *self.face * dot_dir_face;
77        let dir_orthogonal_to_normal = *dir - dir_on_normal;
78        let Some(unit_dir_orthogonal_to_normal) = dir_orthogonal_to_normal.try_normalize(1.0e-6)
79        else {
80            return dot_face >= 0.0;
81        };
82
83        // NOTE: the normalization might be redundant as the result vector is guaranteed to be
84        //       unit sized. Though some rounding errors could throw it off.
85        let Some(adjusted_pseudo_normal) = (edge_on_normal
86            + unit_dir_orthogonal_to_normal * edge_orthogonal_to_normal.norm())
87        .try_normalize(1.0e-6) else {
88            return dot_face >= 0.0;
89        };
90
91        // The reflection of the face normal wrt. the adjusted pseudo-normal gives us the
92        // second end of the pseudo-normal cone the direction is projected on.
93        *dir = adjusted_pseudo_normal * (2.0 * self.face.dot(&adjusted_pseudo_normal)) - *self.face;
94        dot_face >= 0.0
95    }
96}
97
98#[cfg(test)]
99#[cfg(all(feature = "dim3", feature = "alloc"))]
100mod test {
101    use crate::math::{Real, Vector};
102    use crate::shape::TrianglePseudoNormals;
103    use na::Unit;
104
105    use super::NormalConstraints;
106
107    fn bisector(v1: Vector<Real>, v2: Vector<Real>) -> Vector<Real> {
108        (v1 + v2).normalize()
109    }
110
111    fn bisector_y(v: Vector<Real>) -> Vector<Real> {
112        bisector(v, Vector::y())
113    }
114
115    #[test]
116    fn trivial_pseudo_normals_projection() {
117        let pn = TrianglePseudoNormals {
118            face: Vector::y_axis(),
119            edges: [Vector::y_axis(); 3],
120        };
121
122        assert_eq!(
123            pn.project_local_normal(Vector::new(1.0, 1.0, 1.0)),
124            Some(Vector::y())
125        );
126        assert!(pn.project_local_normal(-Vector::y()).is_none());
127    }
128
129    #[test]
130    fn edge_pseudo_normals_projection_strictly_positive() {
131        let bisector = |v1: Vector<Real>, v2: Vector<Real>| (v1 + v2).normalize();
132        let bisector_y = |v: Vector<Real>| bisector(v, Vector::y());
133
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(bisector_y);
141        let cones_axes = cones_ends.map(bisector_y);
142
143        let pn = TrianglePseudoNormals {
144            face: Vector::y_axis(),
145            edges: cones_axes.map(Unit::new_normalize),
146        };
147
148        for i in 0..3 {
149            assert_relative_eq!(
150                pn.project_local_normal(cones_ends[i]).unwrap(),
151                cones_ends[i],
152                epsilon = 1.0e-5
153            );
154            assert_eq!(pn.project_local_normal(cones_axes[i]), Some(cones_axes[i]));
155
156            // Guaranteed to be inside the normal cone of edge i.
157            let subdivs = 100;
158
159            for k in 1..100 {
160                let v = Vector::y()
161                    .lerp(&cones_ends[i], k as Real / (subdivs as Real))
162                    .normalize();
163                assert_eq!(pn.project_local_normal(v).unwrap(), v);
164            }
165
166            // Guaranteed to be outside the normal cone of edge i.
167            for k in 1..subdivs {
168                let v = cones_ref_dir[i]
169                    .lerp(&cones_ends[i], k as Real / (subdivs as Real))
170                    .normalize();
171                assert_relative_eq!(
172                    pn.project_local_normal(v).unwrap(),
173                    cones_ends[i],
174                    epsilon = 1.0e-5
175                );
176            }
177
178            // Guaranteed to be outside the normal cone, and in the -Y half-space.
179            for k in 1..subdivs {
180                let v = cones_ref_dir[i]
181                    .lerp(&(-Vector::y()), k as Real / (subdivs as Real))
182                    .normalize();
183                assert!(pn.project_local_normal(v).is_none(),);
184            }
185        }
186    }
187
188    #[test]
189    fn edge_pseudo_normals_projection_negative() {
190        // The normal cones for this test will be fully contained in the +Y half-space.
191        let cones_ref_dir = [
192            -Vector::z(),
193            -Vector::x(),
194            Vector::new(1.0, 0.0, 1.0).normalize(),
195        ];
196        let cones_ends = cones_ref_dir.map(|v| bisector(v, -Vector::y()));
197        let cones_axes = [
198            bisector(bisector_y(cones_ref_dir[0]), cones_ref_dir[0]),
199            bisector(bisector_y(cones_ref_dir[1]), cones_ref_dir[1]),
200            bisector(bisector_y(cones_ref_dir[2]), cones_ref_dir[2]),
201        ];
202
203        let pn = TrianglePseudoNormals {
204            face: Vector::y_axis(),
205            edges: cones_axes.map(Unit::new_normalize),
206        };
207
208        for i in 0..3 {
209            assert_eq!(pn.project_local_normal(cones_axes[i]), Some(cones_axes[i]));
210
211            // Guaranteed to be inside the normal cone of edge i.
212            let subdivs = 100;
213
214            for k in 1..subdivs {
215                let v = Vector::y()
216                    .lerp(&cones_ends[i], k as Real / (subdivs as Real))
217                    .normalize();
218                assert_eq!(pn.project_local_normal(v).unwrap(), v);
219            }
220
221            // Guaranteed to be outside the normal cone of edge i.
222            // Since it is additionally guaranteed to be in the -Y half-space, we should get None.
223            for k in 1..subdivs {
224                let v = (-Vector::y())
225                    .lerp(&cones_ends[i], k as Real / (subdivs as Real))
226                    .normalize();
227                assert!(pn.project_local_normal(v).is_none());
228            }
229        }
230    }
231}