parry2d/shape/
polygonal_feature2d.rs

1use crate::math::{Isometry, Point, Real, Vector};
2#[cfg(feature = "alloc")]
3use crate::query::{self, ContactManifold, TrackedContact};
4use crate::shape::{PackedFeatureId, Segment};
5
6/// A polygonal feature representing the local polygonal approximation of
7/// a vertex, or face, of a convex shape.
8#[derive(Debug)]
9pub struct PolygonalFeature {
10    /// Up to two vertices forming this polygonal feature.
11    pub vertices: [Point<Real>; 2],
12    /// The feature IDs of this polygon's vertices.
13    pub vids: [PackedFeatureId; 2],
14    /// The feature ID of this polygonal feature.
15    pub fid: PackedFeatureId,
16    /// The number of vertices on this polygon (must be <= 4).
17    pub num_vertices: usize,
18}
19
20impl Default for PolygonalFeature {
21    fn default() -> Self {
22        Self {
23            vertices: [Point::origin(); 2],
24            vids: [PackedFeatureId::UNKNOWN; 2],
25            fid: PackedFeatureId::UNKNOWN,
26            num_vertices: 0,
27        }
28    }
29}
30
31impl From<Segment> for PolygonalFeature {
32    fn from(seg: Segment) -> Self {
33        PolygonalFeature {
34            vertices: [seg.a, seg.b],
35            vids: PackedFeatureId::vertices([0, 2]),
36            fid: PackedFeatureId::face(1),
37            num_vertices: 2,
38        }
39    }
40}
41
42impl PolygonalFeature {
43    /// Transforms the vertices of `self` by the given position `pos`.
44    pub fn transform_by(&mut self, pos: &Isometry<Real>) {
45        self.vertices[0] = pos * self.vertices[0];
46        self.vertices[1] = pos * self.vertices[1];
47    }
48
49    /// Computes the contacts between two polygonal features.
50    #[cfg(feature = "alloc")]
51    pub fn contacts<ManifoldData, ContactData: Default + Copy>(
52        pos12: &Isometry<Real>,
53        pos21: &Isometry<Real>,
54        sep_axis1: &Vector<Real>,
55        sep_axis2: &Vector<Real>,
56        feature1: &Self,
57        feature2: &Self,
58        manifold: &mut ContactManifold<ManifoldData, ContactData>,
59        flipped: bool,
60    ) {
61        match (feature1.num_vertices == 2, feature2.num_vertices == 2) {
62            (true, true) => {
63                Self::face_face_contacts(pos12, feature1, sep_axis1, feature2, manifold, flipped)
64            }
65            (true, false) => {
66                Self::face_vertex_contacts(pos12, feature1, sep_axis1, feature2, manifold, flipped)
67            }
68            (false, true) => {
69                Self::face_vertex_contacts(pos21, feature2, sep_axis2, feature1, manifold, !flipped)
70            }
71            (false, false) => unimplemented!(),
72        }
73    }
74
75    /// Compute contacts points between a face and a vertex.
76    ///
77    /// This method assume we already know that at least one contact exists.
78    #[cfg(feature = "alloc")]
79    pub fn face_vertex_contacts<ManifoldData, ContactData: Default + Copy>(
80        pos12: &Isometry<Real>,
81        face1: &Self,
82        sep_axis1: &Vector<Real>,
83        vertex2: &Self,
84        manifold: &mut ContactManifold<ManifoldData, ContactData>,
85        flipped: bool,
86    ) {
87        let v2_1 = pos12 * vertex2.vertices[0];
88        let tangent1 = face1.vertices[1] - face1.vertices[0];
89        let normal1 = Vector::new(-tangent1.y, tangent1.x);
90        let denom = -normal1.dot(sep_axis1);
91        let dist = (face1.vertices[0] - v2_1).dot(&normal1) / denom;
92        let local_p2 = v2_1;
93        let local_p1 = v2_1 - dist * normal1;
94
95        let contact = TrackedContact::flipped(
96            local_p1,
97            pos12.inverse_transform_point(&local_p2),
98            face1.fid,
99            vertex2.vids[0],
100            dist,
101            flipped,
102        );
103
104        manifold.points.push(contact);
105    }
106
107    /// Computes the contacts between two polygonal faces.
108    #[cfg(feature = "alloc")]
109    pub fn face_face_contacts<ManifoldData, ContactData: Default + Copy>(
110        pos12: &Isometry<Real>,
111        face1: &Self,
112        normal1: &Vector<Real>,
113        face2: &Self,
114        manifold: &mut ContactManifold<ManifoldData, ContactData>,
115        flipped: bool,
116    ) {
117        if let Some((clip_a, clip_b)) = query::details::clip_segment_segment_with_normal(
118            (face1.vertices[0], face1.vertices[1]),
119            (pos12 * face2.vertices[0], pos12 * face2.vertices[1]),
120            *normal1,
121        ) {
122            let fids1 = [face1.vids[0], face1.fid, face1.vids[1]];
123            let fids2 = [face2.vids[0], face2.fid, face2.vids[1]];
124
125            let dist = (clip_a.1 - clip_a.0).dot(normal1);
126            let contact = TrackedContact::flipped(
127                clip_a.0,
128                pos12.inverse_transform_point(&clip_a.1),
129                fids1[clip_a.2],
130                fids2[clip_a.3],
131                dist,
132                flipped,
133            );
134            manifold.points.push(contact);
135
136            let dist = (clip_b.1 - clip_b.0).dot(normal1);
137            let contact = TrackedContact::flipped(
138                clip_b.0,
139                pos12.inverse_transform_point(&clip_b.1),
140                fids1[clip_b.2],
141                fids2[clip_b.3],
142                dist,
143                flipped,
144            );
145            manifold.points.push(contact);
146        }
147    }
148}
parry2d/shape/polygonal_feature2d.rs

parry2d/shape/
polygonal_feature2d.rs
