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