1use crate::math::{Isometry, Point, Vector};
2use crate::query::{self, Contact};
3use crate::shape::{FeatureId, Shape};
4use na::{self, RealField, Unit};
56/// A shape geometry type at the neighborhood of a point.
7#[derive(Copy, Clone, Debug, PartialEq, Eq)]
8pub enum NeighborhoodGeometry<N: RealField + Copy> {
9/// A punctual approximation.
10Point,
11/// A line approximation.
12Line(Unit<Vector<N>>),
13/// A planar approximation.
14Plane(Unit<Vector<N>>),
15}
1617/// The approximation of a shape on the neighborhood of a point.
18#[derive(Copy, Clone, Debug)]
19pub struct LocalShapeApproximation<N: RealField + Copy> {
20// XXX: currently, there is no explicit representation
21 // of the point where the approximation occurs in terms
22 // of shape-specific parameters. That's because we work
23 // so far with polyhedral approximations. Thus, it is
24 // sufficient to known the feature alone to derive an
25 // approximation.
26 // In the future, we might want to:
27 // - Use `parameters` as a set of shape-dependent coordinates giving the location of the
28 // point on it.
29 // - Use `point` as the local-space point where the approximation occurs. It should be
30 // computed by the shape from the parameters.
31/// The shape feature the point lies on.
32pub feature: FeatureId,
33/// The point where approximation is computed.
34pub point: Point<N>,
35/// The approximation geometry.
36pub geometry: NeighborhoodGeometry<N>,
37}
3839impl<N: RealField + Copy> LocalShapeApproximation<N> {
40/// Initializes a new local shape approximation at `point`.
41pub fn new(feature: FeatureId, point: Point<N>, geometry: NeighborhoodGeometry<N>) -> Self {
42 LocalShapeApproximation {
43 feature,
44 point,
45 geometry,
46 }
47 }
48}
4950/// Local contact kinematic of a pair of solids around two given points.
51///
52/// This is used to update the localization of contact points between two solids
53/// from one frame to another. To achieve this, the local shape of the solids
54/// around the given points are approximated by either dilated lines (unbounded
55/// cylinders), planes, dilated points (spheres).
56#[derive(Copy, Clone, Debug)]
57pub struct ContactKinematic<N: RealField + Copy> {
58 approx1: LocalShapeApproximation<N>,
59 approx2: LocalShapeApproximation<N>,
6061 margin1: N,
62 margin2: N,
63}
6465impl<N: RealField + Copy> ContactKinematic<N> {
66/// Initializes an empty contact kinematic.
67 ///
68 /// All the contact kinematic information must be filled using methods
69 /// prefixed by `set_`.
70pub fn new() -> Self {
71let approx = LocalShapeApproximation::new(
72 FeatureId::Unknown,
73 Point::origin(),
74 NeighborhoodGeometry::Point,
75 );
7677 ContactKinematic {
78 margin1: na::zero(),
79 margin2: na::zero(),
80 approx1: approx.clone(),
81 approx2: approx,
82 }
83 }
8485/// Applies the given transformation to the first set of contact information.
86pub fn transform1(&mut self, m: &Isometry<N>) {
87self.approx1.point = m * self.approx1.point;
8889match &mut self.approx1.geometry {
90 NeighborhoodGeometry::Point => {}
91 NeighborhoodGeometry::Plane(n) | NeighborhoodGeometry::Line(n) => {
92*n = m * &*n;
93 }
94 }
95 }
9697/// Applies the given transformation to the second set of contact information.
98pub fn transform2(&mut self, m: &Isometry<N>) {
99self.approx2.point = m * self.approx2.point;
100101match &mut self.approx2.geometry {
102 NeighborhoodGeometry::Point => {}
103 NeighborhoodGeometry::Plane(n) | NeighborhoodGeometry::Line(n) => {
104*n = m * &*n;
105 }
106 }
107 }
108109/// The dilation of the first solid.
110pub fn dilation1(&self) -> N {
111self.margin1
112 }
113114/// The dilation of the second solid.
115pub fn dilation2(&self) -> N {
116self.margin2
117 }
118119/// The tracked point in local space of the first solid.
120 ///
121 /// This may not correspond to the contact point in the local
122 /// space of the first since it does not takes the dilation
123 /// into account.
124// FIXME: we might want to remove this in the future as it is not generalizable to surfaces.
125pub fn local1(&self) -> Point<N> {
126self.approx1.point
127 }
128129/// The tracked point in local space of the second solid.
130 ///
131 /// This may not correspond to the contact point in the local
132 /// space of the second solid since it does not takes the dilation
133 /// into account.
134// FIXME: we might want to remove this in the future as it is not generalizable to surfaces.
135pub fn local2(&self) -> Point<N> {
136self.approx2.point
137 }
138139/// The shape-dependent identifier of the feature of the first solid
140 /// on which lies the contact point.
141pub fn feature1(&self) -> FeatureId {
142self.approx1.feature
143 }
144145/// The shape-dependent identifier of the feature of the second solid
146 /// on which lies the contact point.
147pub fn feature2(&self) -> FeatureId {
148self.approx2.feature
149 }
150151/// Sets the shape-dependent identifier of the feature of the first solid
152 /// on which lies the contact point.
153pub fn set_feature1(&mut self, f: FeatureId) {
154self.approx1.feature = f
155 }
156157/// Sets the shape-dependent identifier of the feature of the second solid
158 /// on which lies the contact point.
159pub fn set_feature2(&mut self, f: FeatureId) {
160self.approx2.feature = f
161 }
162163/// Sets the dilation of the first solid.
164pub fn set_dilation1(&mut self, margin: N) {
165self.margin1 = margin;
166 }
167168/// Sets the dilation of the second solid.
169pub fn set_dilation2(&mut self, margin: N) {
170self.margin2 = margin;
171 }
172173/// The local approximation of the first shape.
174pub fn approx1(&self) -> &LocalShapeApproximation<N> {
175&self.approx1
176 }
177178/// The local approximation of the first shape.
179pub fn approx2(&self) -> &LocalShapeApproximation<N> {
180&self.approx2
181 }
182183/// The local approximation of the first shape.
184pub fn approx1_mut(&mut self) -> &mut LocalShapeApproximation<N> {
185&mut self.approx1
186 }
187188/// The local approximation of the second shape.
189pub fn approx2_mut(&mut self) -> &mut LocalShapeApproximation<N> {
190&mut self.approx2
191 }
192193/// Sets the local approximation of the first shape.
194pub fn set_approx1(
195&mut self,
196 feature: FeatureId,
197 point: Point<N>,
198 geom: NeighborhoodGeometry<N>,
199 ) {
200self.approx1 = LocalShapeApproximation::new(feature, point, geom);
201 }
202203/// Sets the local approximation of the second shape.
204pub fn set_approx2(
205&mut self,
206 feature: FeatureId,
207 point: Point<N>,
208 geom: NeighborhoodGeometry<N>,
209 ) {
210self.approx2 = LocalShapeApproximation::new(feature, point, geom);
211 }
212213/// Computes the updated contact points with the new positions of the solids.
214 ///
215 /// The vector `default_normal1` is the normal of the resulting contact
216 /// in the rare case where the contact normal cannot be determined by the update.
217 /// Typically, this should be set to the latest contact normal known.
218pub fn contact(
219&self,
220 m1: &Isometry<N>,
221 s1: &dyn Shape<N>,
222 deformations1: Option<&[N]>,
223 m2: &Isometry<N>,
224 s2: &dyn Shape<N>,
225 deformations2: Option<&[N]>,
226 default_normal1: &Unit<Vector<N>>,
227 ) -> Option<Contact<N>> {
228let normal;
229let mut depth;
230231let mut world1 = m1 * self.approx1.point;
232let mut world2 = m2 * self.approx2.point;
233234match (&self.approx1.geometry, &self.approx2.geometry) {
235 (NeighborhoodGeometry::Plane(normal1), NeighborhoodGeometry::Point) => {
236 normal = m1 * normal1;
237 depth = -normal.dot(&(world2 - world1));
238 world1 = world2 + *normal * depth;
239 }
240 (NeighborhoodGeometry::Point, NeighborhoodGeometry::Plane(normal2)) => {
241let world_normal2 = m2 * normal2;
242 depth = -world_normal2.dot(&(world1 - world2));
243 world2 = world1 + *world_normal2 * depth;
244 normal = -world_normal2;
245 }
246 (NeighborhoodGeometry::Point, NeighborhoodGeometry::Point) => {
247if let Some((n, d)) = Unit::try_new_and_get(world2 - world1, N::default_epsilon()) {
248if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
249 || s2.tangent_cone_contains_dir(
250self.approx2.feature,
251 m2,
252 deformations2,
253&-n,
254 )
255 {
256 depth = d;
257 normal = -n;
258 } else {
259 depth = -d;
260 normal = n;
261 }
262 } else {
263 depth = na::zero();
264 normal = m1 * default_normal1;
265 }
266 }
267 (NeighborhoodGeometry::Line(dir1), NeighborhoodGeometry::Point) => {
268let world_dir1 = m1 * dir1;
269let mut shift = world2 - world1;
270let proj = world_dir1.dot(&shift);
271 shift -= dir1.into_inner() * proj;
272273if let Some((n, d)) = Unit::try_new_and_get(shift, na::zero()) {
274 world1 = world2 + (-shift);
275276if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
277 || s2.tangent_cone_contains_dir(
278self.approx2.feature,
279 m2,
280 deformations2,
281&-n,
282 )
283 {
284 depth = d;
285 normal = -n;
286 } else {
287 depth = -d;
288 normal = n;
289 }
290 } else {
291 depth = na::zero();
292 normal = m1 * default_normal1;
293 }
294 }
295 (NeighborhoodGeometry::Point, NeighborhoodGeometry::Line(dir2)) => {
296let world_dir2 = m2 * dir2;
297let mut shift = world1 - world2;
298let proj = world_dir2.dot(&shift);
299 shift -= dir2.into_inner() * proj;
300// NOTE: we set:
301 // shift = world2 - world1
302let shift = -shift;
303304if let Some((n, d)) = Unit::try_new_and_get(shift, na::zero()) {
305 world2 = world1 + shift;
306307if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
308 || s2.tangent_cone_contains_dir(
309self.approx2.feature,
310 m2,
311 deformations2,
312&-n,
313 )
314 {
315 depth = d;
316 normal = -n;
317 } else {
318 depth = -d;
319 normal = n;
320 }
321 } else {
322 depth = na::zero();
323 normal = m1 * default_normal1;
324 }
325 }
326 (NeighborhoodGeometry::Line(dir1), NeighborhoodGeometry::Line(dir2)) => {
327let world_dir1 = m1 * dir1;
328let world_dir2 = m2 * dir2;
329let (pt1, pt2) =
330 query::closest_points_line_line(&world1, &world_dir1, &world2, &world_dir2);
331332 world1 = pt1;
333 world2 = pt2;
334335if let Some((n, d)) = Unit::try_new_and_get(world2 - world1, na::zero()) {
336if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
337 || s2.tangent_cone_contains_dir(
338self.approx2.feature,
339 m2,
340 deformations2,
341&-n,
342 )
343 {
344 depth = d;
345 normal = -n;
346 } else {
347 depth = -d;
348 normal = n;
349 }
350 } else {
351 depth = na::zero();
352 normal = m1 * default_normal1;
353 }
354 }
355_ => {
356return None;
357 }
358 }
359360 world1 += normal.into_inner() * self.margin1;
361 world2 += normal.into_inner() * (-self.margin2);
362 depth += self.margin1 + self.margin2;
363364Some(Contact::new(world1, world2, normal, depth))
365 }
366}