parry3d/shape/
polygonal_feature3d.rs

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/// A polygonal feature representing the local polygonal approximation of
10/// a vertex, face, or edge of a convex shape.
11#[derive(Debug, Clone)]
12pub struct PolygonalFeature {
13    /// Up to four vertices forming this polygonal feature.
14    pub vertices: [Point<Real>; 4],
15    /// The feature IDs of this polygon's vertices.
16    pub vids: [PackedFeatureId; 4],
17    /// The feature IDs of this polygon's edges.
18    pub eids: [PackedFeatureId; 4],
19    /// The feature ID of this polygonal feature.
20    pub fid: PackedFeatureId,
21    /// The number of vertices on this polygon (must be <= 4).
22    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    /// Creates a new empty polygonal feature.
63    pub fn new() -> Self {
64        Self::default()
65    }
66
67    /// Transform each vertex of this polygonal feature by the given position `pos`.
68    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    /// Computes all the contacts between two polygonal features.
75    #[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        // Project the faces to a 2D plane for contact clipping.
104        // The plane they are projected onto has normal sep_axis1
105        // and contains the origin (this is numerically OK because
106        // we are not working in world-space here).
107        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        // TODO: not sure what the best value for eps is.
137        // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small.
138        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                // Found a contact between the two edges.
150                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        // The lines are parallel so we are having a conformal contact.
173        // Let's use a range-based clipping to extract two contact points.
174        // TODO: would it be better and/or more efficient to do the
175        //clipping in 2D?
176        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        // Project the faces to a 2D plane for contact clipping.
217        // The plane they are projected onto has normal sep_axis1
218        // and contains the origin (this is numerically OK because
219        // we are not working in world-space here).
220        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        // Also find all the vertices located inside of the other projected face.
266        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)] // Would make it much more verbose.
275                '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                            // The point lies outside.
288                            continue 'point_loop1;
289                        }
290                    }
291
292                    // All the perp had the same sign: the point is inside of the other shapes projection.
293                    // Output the contact.
294                    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                            // The point lies outside.
330                            continue 'point_loop2;
331                        }
332                    }
333
334                    // All the perp had the same sign: the point is inside of the other shapes projection.
335                    // Output the contact.
336                    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        // Now we have to compute the intersection between all pairs of
353        // edges from the face 1 and from the face2.
354        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                        // Found a contact between the two edges.
368                        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
392/// Compute the barycentric coordinates of the intersection between the two given lines.
393/// Returns `None` if the lines are parallel.
394fn closest_points_line2d(
395    edge1: [Point2<Real>; 2],
396    edge2: [Point2<Real>; 2],
397) -> Option<(Real, Real)> {
398    // Inspired by Real-time collision detection by Christer Ericson.
399    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            // Use absolute and ulps error to test collinearity.
424            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}