parry2d/shape/
polygonal_feature_map.rs

1use crate::math::Vector;
2#[cfg(feature = "dim3")]
3use crate::shape::{Cone, Cylinder, PackedFeatureId};
4use crate::shape::{Cuboid, PolygonalFeature, Segment, SupportMap, Triangle};
5#[cfg(feature = "dim3")]
6use crate::utils::WSign;
7
8/// Trait implemented by convex shapes with features with polyhedral approximations.
9pub trait PolygonalFeatureMap: SupportMap {
10    /// Compute the support polygonal face of `self` towards the `dir`.
11    fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature);
12
13    /// Compute the support polygonal face of `self` towards `dir`, oriented so that it best
14    /// covers the neighborhood of the local-space point `hint`.
15    ///
16    /// The default implementation ignores `hint`, which is exact for genuine polygons.
17    /// Shapes approximating a curved feature (like a cylinder cap by an inscribed square)
18    /// must orient it from `hint`: a fixed azimuth misses up to ~36% of the cap near its
19    /// rim, leaving contacts there with a single unclippable point.
20    fn local_support_feature_toward(
21        &self,
22        dir: Vector,
23        hint: Vector,
24        out_feature: &mut PolygonalFeature,
25    ) {
26        let _ = hint;
27        self.local_support_feature(dir, out_feature);
28    }
29
30    // TODO: this is currently just a workaround for https://github.com/dimforge/rapier/issues/417
31    //       until we get a better way to deal with the issue without breaking internal edges
32    //       handling.
33    /// Is this shape a `ConvexPolyhedron`?
34    fn is_convex_polyhedron(&self) -> bool {
35        false
36    }
37}
38
39impl PolygonalFeatureMap for Segment {
40    fn local_support_feature(&self, _: Vector, out_feature: &mut PolygonalFeature) {
41        *out_feature = PolygonalFeature::from(*self);
42    }
43}
44
45impl PolygonalFeatureMap for Triangle {
46    fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature) {
47        *out_feature = self.support_face(dir);
48    }
49}
50
51impl PolygonalFeatureMap for Cuboid {
52    fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature) {
53        *out_feature = self.support_face(dir);
54    }
55}
56
57/// Azimuth (unit `xz` direction) orienting the polygonal approximation of a cylinder's or
58/// cone's curved features, taken from `hint` if it lies off the axis, else from `dir`.
59#[cfg(feature = "dim3")]
60fn feature_azimuth(dir: Vector, hint: Option<Vector>) -> crate::math::Vector2 {
61    use crate::math::Vector2;
62    hint.and_then(|hint| Vector2::new(hint.x, hint.z).try_normalize())
63        .or_else(|| Vector2::new(dir.x, dir.z).try_normalize())
64        .unwrap_or(Vector2::X)
65}
66
67#[cfg(feature = "dim3")]
68impl PolygonalFeatureMap for Cylinder {
69    fn local_support_feature(&self, dir: Vector, out_features: &mut PolygonalFeature) {
70        self.support_feature_with_azimuth(dir, feature_azimuth(dir, None), out_features);
71    }
72
73    fn local_support_feature_toward(
74        &self,
75        dir: Vector,
76        hint: Vector,
77        out_features: &mut PolygonalFeature,
78    ) {
79        // Orienting the cap's square approximation so that one of its vertices (which lie on
80        // the cap's rim) sits at the contact's azimuth guarantees the approximation covers
81        // the cap's surface in the contact's neighborhood, wherever it lies on the cap.
82        self.support_feature_with_azimuth(dir, feature_azimuth(dir, Some(hint)), out_features);
83    }
84}
85
86#[cfg(feature = "dim3")]
87impl Cylinder {
88    fn support_feature_with_azimuth(
89        &self,
90        dir: Vector,
91        dir2: crate::math::Vector2,
92        out_features: &mut PolygonalFeature,
93    ) {
94        // About feature ids.
95        // At all times, we consider our cylinder to be approximated as follows:
96        // - The curved part is approximated by a single segment.
97        // - Each flat cap of the cylinder is approximated by a square.
98        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
99        //   `y` coordinate has an ID of 1.
100        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
101        //   when looking at the cap with an eye looking towards +y).
102        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
103        // - The bottom cap has its face feature ID of 9.
104        // - The feature IDs of the top cap are the same as the bottom cap to which we add 10.
105        //   So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19.
106        // - Note that at all times, one of each cap's vertices are the same as the curved-part
107        //   segment endpoints.
108        if dir.y.abs() < 0.5 {
109            // We return a segment lying on the cylinder's curved part.
110            out_features.vertices[0] = Vector::new(
111                dir2.x * self.radius,
112                -self.half_height,
113                dir2.y * self.radius,
114            );
115            out_features.vertices[1] =
116                Vector::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
117            out_features.eids = PackedFeatureId::edges([0, 0, 0, 0]);
118            out_features.fid = PackedFeatureId::face(0);
119            out_features.num_vertices = 2;
120            out_features.vids = PackedFeatureId::vertices([1, 11, 11, 11]);
121        } else {
122            // We return a square approximation of the cylinder cap.
123            let y = dir.y.copy_sign_to(self.half_height);
124            out_features.vertices[0] = Vector::new(dir2.x * self.radius, y, dir2.y * self.radius);
125            out_features.vertices[1] = Vector::new(-dir2.y * self.radius, y, dir2.x * self.radius);
126            out_features.vertices[2] = Vector::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
127            out_features.vertices[3] = Vector::new(dir2.y * self.radius, y, -dir2.x * self.radius);
128
129            if dir.y < 0.0 {
130                out_features.eids = PackedFeatureId::edges([2, 4, 6, 8]);
131                out_features.fid = PackedFeatureId::face(9);
132                out_features.num_vertices = 4;
133                out_features.vids = PackedFeatureId::vertices([1, 3, 5, 7]);
134            } else {
135                out_features.eids = PackedFeatureId::edges([12, 14, 16, 18]);
136                out_features.fid = PackedFeatureId::face(19);
137                out_features.num_vertices = 4;
138                out_features.vids = PackedFeatureId::vertices([11, 13, 15, 17]);
139            }
140        }
141    }
142}
143
144#[cfg(feature = "dim3")]
145impl PolygonalFeatureMap for Cone {
146    fn local_support_feature(&self, dir: Vector, out_features: &mut PolygonalFeature) {
147        self.support_feature_with_azimuth(dir, feature_azimuth(dir, None), out_features);
148    }
149
150    fn local_support_feature_toward(
151        &self,
152        dir: Vector,
153        hint: Vector,
154        out_features: &mut PolygonalFeature,
155    ) {
156        // See the comment in the cylinder's implementation.
157        self.support_feature_with_azimuth(dir, feature_azimuth(dir, Some(hint)), out_features);
158    }
159}
160
161#[cfg(feature = "dim3")]
162impl Cone {
163    fn support_feature_with_azimuth(
164        &self,
165        dir: Vector,
166        dir2: crate::math::Vector2,
167        out_features: &mut PolygonalFeature,
168    ) {
169        // About feature ids. It is very similar to the feature ids of cylinders.
170        // At all times, we consider our cone to be approximated as follows:
171        // - The curved part is approximated by a single segment.
172        // - The flat cap of the cone is approximated by a square.
173        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
174        //   `y` coordinate has an ID of 1.
175        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
176        //   when looking at the cap with an eye looking towards +y).
177        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
178        // - The bottom cap has its face feature ID of 9.
179        // - Note that at all times, one of the cap's vertices are the same as the curved-part
180        //   segment endpoints.
181        if dir.y > 0.0 {
182            // We return a segment lying on the cone's curved part.
183            out_features.vertices[0] = Vector::new(
184                dir2.x * self.radius,
185                -self.half_height,
186                dir2.y * self.radius,
187            );
188            out_features.vertices[1] = Vector::new(0.0, self.half_height, 0.0);
189            out_features.eids = PackedFeatureId::edges([0, 0, 0, 0]);
190            out_features.fid = PackedFeatureId::face(0);
191            out_features.num_vertices = 2;
192            out_features.vids = PackedFeatureId::vertices([1, 11, 11, 11]);
193        } else {
194            // We return a square approximation of the cone cap.
195            let y = -self.half_height;
196            out_features.vertices[0] = Vector::new(dir2.x * self.radius, y, dir2.y * self.radius);
197            out_features.vertices[1] = Vector::new(-dir2.y * self.radius, y, dir2.x * self.radius);
198            out_features.vertices[2] = Vector::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
199            out_features.vertices[3] = Vector::new(dir2.y * self.radius, y, -dir2.x * self.radius);
200
201            out_features.eids = PackedFeatureId::edges([2, 4, 6, 8]);
202            out_features.fid = PackedFeatureId::face(9);
203            out_features.num_vertices = 4;
204            out_features.vids = PackedFeatureId::vertices([1, 3, 5, 7]);
205        }
206    }
207}
parry2d/shape/polygonal_feature_map.rs

parry2d/shape/
polygonal_feature_map.rs
