parry3d/shape/
triangle_pseudo_normals.rs1#[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#[derive(Clone, Debug)]
23pub struct TrianglePseudoNormals {
24 pub face: UnitVector<Real>,
26 pub edges: [UnitVector<Real>; 3],
32}
33
34#[cfg(feature = "alloc")]
35impl NormalConstraints for TrianglePseudoNormals {
36 fn project_local_normal_mut(&self, dir: &mut Vector<Real>) -> bool {
39 let dot_face = dir.dot(&self.face);
40
41 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 if *closest_edge == self.face {
56 *dir = *self.face;
58 return dot_face >= 0.0;
59 }
60
61 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; if dot_dir_face >= dot_corrected_dir_face {
68 return true;
70 }
71
72 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 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 *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 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 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 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 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 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 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 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}