1use crate::approx::AbsDiffEq;
2use crate::math::{Isometry, Point, Real, Vector};
3#[cfg(feature = "alloc")]
4use crate::query::{ContactManifold, TrackedContact};
5use crate::shape::{PackedFeatureId, Segment, Triangle};
6use crate::utils::WBasis;
7use na::Point2;
8
9#[derive(Debug, Clone)]
12pub struct PolygonalFeature {
13 pub vertices: [Point<Real>; 4],
15 pub vids: [PackedFeatureId; 4],
17 pub eids: [PackedFeatureId; 4],
19 pub fid: PackedFeatureId,
21 pub num_vertices: usize,
23}
24
25impl Default for PolygonalFeature {
26 fn default() -> Self {
27 Self {
28 vertices: [Point::origin(); 4],
29 vids: [PackedFeatureId::UNKNOWN; 4],
30 eids: [PackedFeatureId::UNKNOWN; 4],
31 fid: PackedFeatureId::UNKNOWN,
32 num_vertices: 0,
33 }
34 }
35}
36
37impl From<Triangle> for PolygonalFeature {
38 fn from(tri: Triangle) -> Self {
39 Self {
40 vertices: [tri.a, tri.b, tri.c, tri.c],
41 vids: PackedFeatureId::vertices([0, 1, 2, 2]),
42 eids: PackedFeatureId::edges([0, 1, 2, 2]),
43 fid: PackedFeatureId::face(0),
44 num_vertices: 3,
45 }
46 }
47}
48
49impl From<Segment> for PolygonalFeature {
50 fn from(seg: Segment) -> Self {
51 Self {
52 vertices: [seg.a, seg.b, seg.b, seg.b],
53 vids: PackedFeatureId::vertices([0, 1, 1, 1]),
54 eids: PackedFeatureId::edges([0, 0, 0, 0]),
55 fid: PackedFeatureId::face(0),
56 num_vertices: 2,
57 }
58 }
59}
60
61impl PolygonalFeature {
62 pub fn new() -> Self {
64 Self::default()
65 }
66
67 pub fn transform_by(&mut self, pos: &Isometry<Real>) {
69 for p in &mut self.vertices[0..self.num_vertices] {
70 *p = pos * *p;
71 }
72 }
73
74 #[cfg(feature = "alloc")]
76 pub fn contacts<ManifoldData, ContactData: Default + Copy>(
77 pos12: &Isometry<Real>,
78 _pos21: &Isometry<Real>,
79 sep_axis1: &Vector<Real>,
80 _sep_axis2: &Vector<Real>,
81 feature1: &Self,
82 feature2: &Self,
83 manifold: &mut ContactManifold<ManifoldData, ContactData>,
84 flipped: bool,
85 ) {
86 match (feature1.num_vertices, feature2.num_vertices) {
87 (2, 2) => {
88 Self::contacts_edge_edge(pos12, feature1, sep_axis1, feature2, manifold, flipped)
89 }
90 _ => Self::contacts_face_face(pos12, feature1, sep_axis1, feature2, manifold, flipped),
91 }
92 }
93
94 #[cfg(feature = "alloc")]
95 fn contacts_edge_edge<ManifoldData, ContactData: Default + Copy>(
96 pos12: &Isometry<Real>,
97 face1: &PolygonalFeature,
98 sep_axis1: &Vector<Real>,
99 face2: &PolygonalFeature,
100 manifold: &mut ContactManifold<ManifoldData, ContactData>,
101 flipped: bool,
102 ) {
103 let basis = sep_axis1.orthonormal_basis();
108 let projected_edge1 = [
109 Point2::new(
110 face1.vertices[0].coords.dot(&basis[0]),
111 face1.vertices[0].coords.dot(&basis[1]),
112 ),
113 Point2::new(
114 face1.vertices[1].coords.dot(&basis[0]),
115 face1.vertices[1].coords.dot(&basis[1]),
116 ),
117 ];
118
119 let vertices2_1 = [pos12 * face2.vertices[0], pos12 * face2.vertices[1]];
120 let projected_edge2 = [
121 Point2::new(
122 vertices2_1[0].coords.dot(&basis[0]),
123 vertices2_1[0].coords.dot(&basis[1]),
124 ),
125 Point2::new(
126 vertices2_1[1].coords.dot(&basis[0]),
127 vertices2_1[1].coords.dot(&basis[1]),
128 ),
129 ];
130
131 let tangent1 =
132 (projected_edge1[1] - projected_edge1[0]).try_normalize(Real::default_epsilon());
133 let tangent2 =
134 (projected_edge2[1] - projected_edge2[0]).try_normalize(Real::default_epsilon());
135
136 if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) {
139 let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8;
140
141 if !parallel {
142 let seg1 = (&projected_edge1[0], &projected_edge1[1]);
143 let seg2 = (&projected_edge2[0], &projected_edge2[1]);
144 let (loc1, loc2) =
145 crate::query::details::closest_points_segment_segment_with_locations_nD(
146 seg1, seg2,
147 );
148
149 let bcoords1 = loc1.barycentric_coordinates();
151 let bcoords2 = loc2.barycentric_coordinates();
152
153 let edge1 = (face1.vertices[0], face1.vertices[1]);
154 let edge2 = (vertices2_1[0], vertices2_1[1]);
155 let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1];
156 let local_p2_1 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1];
157 let dist = (local_p2_1 - local_p1).dot(sep_axis1);
158
159 manifold.points.push(TrackedContact::flipped(
160 local_p1,
161 pos12.inverse_transform_point(&local_p2_1),
162 face1.eids[0],
163 face2.eids[0],
164 dist,
165 flipped,
166 ));
167
168 return;
169 }
170 }
171
172 if let Some(clips) = crate::query::details::clip_segment_segment(
177 (face1.vertices[0], face1.vertices[1]),
178 (vertices2_1[0], vertices2_1[1]),
179 ) {
180 let feature_at = |face: &PolygonalFeature, id| match id {
181 0 => face.vids[0],
182 1 => face.eids[0],
183 2 => face.vids[1],
184 _ => unreachable!(),
185 };
186
187 manifold.points.push(TrackedContact::flipped(
188 (clips.0).0,
189 pos12.inverse_transform_point(&(clips.0).1),
190 feature_at(face1, (clips.0).2),
191 feature_at(face2, (clips.0).3),
192 ((clips.0).1 - (clips.0).0).dot(sep_axis1),
193 flipped,
194 ));
195
196 manifold.points.push(TrackedContact::flipped(
197 (clips.1).0,
198 pos12.inverse_transform_point(&(clips.1).1),
199 feature_at(face1, (clips.1).2),
200 feature_at(face2, (clips.1).3),
201 ((clips.1).1 - (clips.1).0).dot(sep_axis1),
202 flipped,
203 ));
204 }
205 }
206
207 #[cfg(feature = "alloc")]
208 fn contacts_face_face<ManifoldData, ContactData: Default + Copy>(
209 pos12: &Isometry<Real>,
210 face1: &PolygonalFeature,
211 sep_axis1: &Vector<Real>,
212 face2: &PolygonalFeature,
213 manifold: &mut ContactManifold<ManifoldData, ContactData>,
214 flipped: bool,
215 ) {
216 let basis = sep_axis1.orthonormal_basis();
221 let projected_face1 = [
222 Point2::new(
223 face1.vertices[0].coords.dot(&basis[0]),
224 face1.vertices[0].coords.dot(&basis[1]),
225 ),
226 Point2::new(
227 face1.vertices[1].coords.dot(&basis[0]),
228 face1.vertices[1].coords.dot(&basis[1]),
229 ),
230 Point2::new(
231 face1.vertices[2].coords.dot(&basis[0]),
232 face1.vertices[2].coords.dot(&basis[1]),
233 ),
234 Point2::new(
235 face1.vertices[3].coords.dot(&basis[0]),
236 face1.vertices[3].coords.dot(&basis[1]),
237 ),
238 ];
239
240 let vertices2_1 = [
241 pos12 * face2.vertices[0],
242 pos12 * face2.vertices[1],
243 pos12 * face2.vertices[2],
244 pos12 * face2.vertices[3],
245 ];
246 let projected_face2 = [
247 Point2::new(
248 vertices2_1[0].coords.dot(&basis[0]),
249 vertices2_1[0].coords.dot(&basis[1]),
250 ),
251 Point2::new(
252 vertices2_1[1].coords.dot(&basis[0]),
253 vertices2_1[1].coords.dot(&basis[1]),
254 ),
255 Point2::new(
256 vertices2_1[2].coords.dot(&basis[0]),
257 vertices2_1[2].coords.dot(&basis[1]),
258 ),
259 Point2::new(
260 vertices2_1[3].coords.dot(&basis[0]),
261 vertices2_1[3].coords.dot(&basis[1]),
262 ),
263 ];
264
265 if face2.num_vertices > 2 {
267 let normal2_1 =
268 (vertices2_1[2] - vertices2_1[1]).cross(&(vertices2_1[0] - vertices2_1[1]));
269 let denom = normal2_1.dot(sep_axis1);
270
271 if !relative_eq!(denom, 0.0) {
272 let last_index2 = face2.num_vertices - 1;
273
274 #[allow(clippy::needless_range_loop)] 'point_loop1: for i in 0..face1.num_vertices {
276 let p1 = projected_face1[i];
277
278 let mut sign = (projected_face2[0] - projected_face2[last_index2])
279 .perp(&(p1 - projected_face2[last_index2]));
280 for j in 0..last_index2 {
281 let new_sign = (projected_face2[j + 1] - projected_face2[j])
282 .perp(&(p1 - projected_face2[j]));
283
284 if sign == 0.0 {
285 sign = new_sign;
286 } else if sign * new_sign < 0.0 {
287 continue 'point_loop1;
289 }
290 }
291
292 let dist = (vertices2_1[0] - face1.vertices[i]).dot(&normal2_1) / denom;
295 let local_p1 = face1.vertices[i];
296 let local_p2_1 = face1.vertices[i] + dist * sep_axis1;
297
298 manifold.points.push(TrackedContact::flipped(
299 local_p1,
300 pos12.inverse_transform_point(&local_p2_1),
301 face1.vids[i],
302 face2.fid,
303 dist,
304 flipped,
305 ));
306 }
307 }
308 }
309
310 if face1.num_vertices > 2 {
311 let normal1 = (face1.vertices[2] - face1.vertices[1])
312 .cross(&(face1.vertices[0] - face1.vertices[1]));
313
314 let denom = -normal1.dot(sep_axis1);
315 if !relative_eq!(denom, 0.0) {
316 let last_index1 = face1.num_vertices - 1;
317 'point_loop2: for i in 0..face2.num_vertices {
318 let p2 = projected_face2[i];
319
320 let mut sign = (projected_face1[0] - projected_face1[last_index1])
321 .perp(&(p2 - projected_face1[last_index1]));
322 for j in 0..last_index1 {
323 let new_sign = (projected_face1[j + 1] - projected_face1[j])
324 .perp(&(p2 - projected_face1[j]));
325
326 if sign == 0.0 {
327 sign = new_sign;
328 } else if sign * new_sign < 0.0 {
329 continue 'point_loop2;
331 }
332 }
333
334 let dist = (face1.vertices[0] - vertices2_1[i]).dot(&normal1) / denom;
337 let local_p2_1 = vertices2_1[i];
338 let local_p1 = vertices2_1[i] - dist * sep_axis1;
339
340 manifold.points.push(TrackedContact::flipped(
341 local_p1,
342 pos12.inverse_transform_point(&local_p2_1),
343 face1.fid,
344 face2.vids[i],
345 dist,
346 flipped,
347 ));
348 }
349 }
350 }
351
352 for j in 0..face2.num_vertices {
355 let projected_edge2 = [
356 projected_face2[j],
357 projected_face2[(j + 1) % face2.num_vertices],
358 ];
359
360 for i in 0..face1.num_vertices {
361 let projected_edge1 = [
362 projected_face1[i],
363 projected_face1[(i + 1) % face1.num_vertices],
364 ];
365 if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
366 if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 {
367 let edge1 = (
369 face1.vertices[i],
370 face1.vertices[(i + 1) % face1.num_vertices],
371 );
372 let edge2 = (vertices2_1[j], vertices2_1[(j + 1) % face2.num_vertices]);
373 let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
374 let local_p2_1 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1;
375 let dist = (local_p2_1 - local_p1).dot(sep_axis1);
376
377 manifold.points.push(TrackedContact::flipped(
378 local_p1,
379 pos12.inverse_transform_point(&local_p2_1),
380 face1.eids[i],
381 face2.eids[j],
382 dist,
383 flipped,
384 ));
385 }
386 }
387 }
388 }
389 }
390}
391
392fn closest_points_line2d(
395 edge1: [Point2<Real>; 2],
396 edge2: [Point2<Real>; 2],
397) -> Option<(Real, Real)> {
398 let dir1 = edge1[1] - edge1[0];
400 let dir2 = edge2[1] - edge2[0];
401 let r = edge1[0] - edge2[0];
402
403 let a = dir1.norm_squared();
404 let e = dir2.norm_squared();
405 let f = dir2.dot(&r);
406
407 let eps = Real::default_epsilon();
408
409 if a <= eps && e <= eps {
410 Some((0.0, 0.0))
411 } else if a <= eps {
412 Some((0.0, f / e))
413 } else {
414 let c = dir1.dot(&r);
415 if e <= eps {
416 Some((-c / a, 0.0))
417 } else {
418 let b = dir1.dot(&dir2);
419 let ae = a * e;
420 let bb = b * b;
421 let denom = ae - bb;
422
423 let parallel = denom <= eps || approx::ulps_eq!(ae, bb);
425
426 let s = if !parallel {
427 (b * f - c * e) / denom
428 } else {
429 0.0
430 };
431
432 if parallel {
433 None
434 } else {
435 Some((s, (b * s + f) / e))
436 }
437 }
438 }
439}