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
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
use crate::math::{Isometry, Point, Vector};
use crate::query::{self, Contact};
use crate::shape::{FeatureId, Shape};
use na::{self, RealField, Unit};

/// A shape geometry type at the neighborhood of a point.
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub enum NeighborhoodGeometry<N: RealField> {
    /// A punctual approximation.
    Point,
    /// A line approximation.
    Line(Unit<Vector<N>>),
    /// A planar approximation.
    Plane(Unit<Vector<N>>),
}

/// The approximation of a shape on the neighborhood of a point.
#[derive(Copy, Clone, Debug)]
pub struct LocalShapeApproximation<N: RealField> {
    // XXX: currently, there is no explicit representation
    // of the point where the approximation occurs in terms
    // of shape-specific parameters. That's because we work
    // so far with polyhedral approximations. Thus, it is
    // sufficient to known the feature alone to derive an
    // approximation.
    // In the future, we might want to:
    // - Use `parameters` as a set of shape-dependent coordinates giving the location of the
    //   point on it.
    // - Use `point` as the local-space point where the approximation occurs. It should be
    //   computed by the shape from the parameters.
    /// The shape feature the point lies on.
    pub feature: FeatureId,
    /// The point where approximation is computed.
    pub point: Point<N>,
    /// The approximation geometry.
    pub geometry: NeighborhoodGeometry<N>,
}

impl<N: RealField> LocalShapeApproximation<N> {
    /// Initializes a new local shape approximation at `point`.
    pub fn new(feature: FeatureId, point: Point<N>, geometry: NeighborhoodGeometry<N>) -> Self {
        LocalShapeApproximation {
            feature,
            point,
            geometry,
        }
    }
}

/// Local contact kinematic of a pair of solids around two given points.
///
/// This is used to update the localization of contact points between two solids
/// from one frame to another. To achieve this, the local shape of the solids
/// around the given points are approximated by either dilated lines (unbounded
/// cylinders), planes, dilated points (spheres).
#[derive(Copy, Clone, Debug)]
pub struct ContactKinematic<N: RealField> {
    approx1: LocalShapeApproximation<N>,
    approx2: LocalShapeApproximation<N>,

    margin1: N,
    margin2: N,
}

impl<N: RealField> ContactKinematic<N> {
    /// Initializes an empty contact kinematic.
    ///
    /// All the contact kinematic information must be filled using methods
    /// prefixed by `set_`.
    pub fn new() -> Self {
        let approx = LocalShapeApproximation::new(
            FeatureId::Unknown,
            Point::origin(),
            NeighborhoodGeometry::Point,
        );

        ContactKinematic {
            margin1: na::zero(),
            margin2: na::zero(),
            approx1: approx.clone(),
            approx2: approx,
        }
    }

    /// Applies the given transformation to the first set of contact information.
    pub fn transform1(&mut self, m: &Isometry<N>) {
        self.approx1.point = m * self.approx1.point;

        match &mut self.approx1.geometry {
            NeighborhoodGeometry::Point => {}
            NeighborhoodGeometry::Plane(n) | NeighborhoodGeometry::Line(n) => {
                *n = m * &*n;
            }
        }
    }

    /// Applies the given transformation to the second set of contact information.
    pub fn transform2(&mut self, m: &Isometry<N>) {
        self.approx2.point = m * self.approx2.point;

        match &mut self.approx2.geometry {
            NeighborhoodGeometry::Point => {}
            NeighborhoodGeometry::Plane(n) | NeighborhoodGeometry::Line(n) => {
                *n = m * &*n;
            }
        }
    }

    /// The dilation of the first solid.
    pub fn dilation1(&self) -> N {
        self.margin1
    }

    /// The dilation of the second solid.
    pub fn dilation2(&self) -> N {
        self.margin2
    }

    /// The tracked point in local space of the first solid.
    ///
    /// This may not correspond to the contact point in the local
    /// space of the first since it does not takes the dilation
    /// into account.
    // FIXME: we might want to remove this in the future as it is not generalizable to surfaces.
    pub fn local1(&self) -> Point<N> {
        self.approx1.point
    }

    /// The tracked point in local space of the second solid.
    ///
    /// This may not correspond to the contact point in the local
    /// space of the second solid since it does not takes the dilation
    /// into account.
    // FIXME: we might want to remove this in the future as it is not generalizable to surfaces.
    pub fn local2(&self) -> Point<N> {
        self.approx2.point
    }

    /// The shape-dependent identifier of the feature of the first solid
    /// on which lies the contact point.
    pub fn feature1(&self) -> FeatureId {
        self.approx1.feature
    }

    /// The shape-dependent identifier of the feature of the second solid
    /// on which lies the contact point.
    pub fn feature2(&self) -> FeatureId {
        self.approx2.feature
    }

    /// Sets the shape-dependent identifier of the feature of the first solid
    /// on which lies the contact point.
    pub fn set_feature1(&mut self, f: FeatureId) {
        self.approx1.feature = f
    }

    /// Sets the shape-dependent identifier of the feature of the second solid
    /// on which lies the contact point.
    pub fn set_feature2(&mut self, f: FeatureId) {
        self.approx2.feature = f
    }

    /// Sets the dilation of the first solid.
    pub fn set_dilation1(&mut self, margin: N) {
        self.margin1 = margin;
    }

    /// Sets the dilation of the second solid.
    pub fn set_dilation2(&mut self, margin: N) {
        self.margin2 = margin;
    }

    /// The local approximation of the first shape.
    pub fn approx1(&self) -> &LocalShapeApproximation<N> {
        &self.approx1
    }

    /// The local approximation of the first shape.
    pub fn approx2(&self) -> &LocalShapeApproximation<N> {
        &self.approx2
    }

    /// The local approximation of the first shape.
    pub fn approx1_mut(&mut self) -> &mut LocalShapeApproximation<N> {
        &mut self.approx1
    }

    /// The local approximation of the second shape.
    pub fn approx2_mut(&mut self) -> &mut LocalShapeApproximation<N> {
        &mut self.approx2
    }

    /// Sets the local approximation of the first shape.
    pub fn set_approx1(
        &mut self,
        feature: FeatureId,
        point: Point<N>,
        geom: NeighborhoodGeometry<N>,
    ) {
        self.approx1 = LocalShapeApproximation::new(feature, point, geom);
    }

    /// Sets the local approximation of the second shape.
    pub fn set_approx2(
        &mut self,
        feature: FeatureId,
        point: Point<N>,
        geom: NeighborhoodGeometry<N>,
    ) {
        self.approx2 = LocalShapeApproximation::new(feature, point, geom);
    }

    /// Computes the updated contact points with the new positions of the solids.
    ///
    /// The vector `default_normal1` is the normal of the resulting contact
    /// in the rare case where the contact normal cannot be determined by the update.
    /// Typically, this should be set to the latest contact normal known.
    pub fn contact(
        &self,
        m1: &Isometry<N>,
        s1: &dyn Shape<N>,
        deformations1: Option<&[N]>,
        m2: &Isometry<N>,
        s2: &dyn Shape<N>,
        deformations2: Option<&[N]>,
        default_normal1: &Unit<Vector<N>>,
    ) -> Option<Contact<N>> {
        let normal;
        let mut depth;

        let mut world1 = m1 * self.approx1.point;
        let mut world2 = m2 * self.approx2.point;

        match (&self.approx1.geometry, &self.approx2.geometry) {
            (NeighborhoodGeometry::Plane(normal1), NeighborhoodGeometry::Point) => {
                normal = m1 * normal1;
                depth = -normal.dot(&(world2 - world1));
                world1 = world2 + *normal * depth;
            }
            (NeighborhoodGeometry::Point, NeighborhoodGeometry::Plane(normal2)) => {
                let world_normal2 = m2 * normal2;
                depth = -world_normal2.dot(&(world1 - world2));
                world2 = world1 + *world_normal2 * depth;
                normal = -world_normal2;
            }
            (NeighborhoodGeometry::Point, NeighborhoodGeometry::Point) => {
                if let Some((n, d)) = Unit::try_new_and_get(world2 - world1, N::default_epsilon()) {
                    if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
                        || s2.tangent_cone_contains_dir(
                            self.approx2.feature,
                            m2,
                            deformations2,
                            &-n,
                        )
                    {
                        depth = d;
                        normal = -n;
                    } else {
                        depth = -d;
                        normal = n;
                    }
                } else {
                    depth = na::zero();
                    normal = m1 * default_normal1;
                }
            }
            (NeighborhoodGeometry::Line(dir1), NeighborhoodGeometry::Point) => {
                let world_dir1 = m1 * dir1;
                let mut shift = world2 - world1;
                let proj = world_dir1.dot(&shift);
                shift -= dir1.into_inner() * proj;

                if let Some((n, d)) = Unit::try_new_and_get(shift, na::zero()) {
                    world1 = world2 + (-shift);

                    if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
                        || s2.tangent_cone_contains_dir(
                            self.approx2.feature,
                            m2,
                            deformations2,
                            &-n,
                        )
                    {
                        depth = d;
                        normal = -n;
                    } else {
                        depth = -d;
                        normal = n;
                    }
                } else {
                    depth = na::zero();
                    normal = m1 * default_normal1;
                }
            }
            (NeighborhoodGeometry::Point, NeighborhoodGeometry::Line(dir2)) => {
                let world_dir2 = m2 * dir2;
                let mut shift = world1 - world2;
                let proj = world_dir2.dot(&shift);
                shift -= dir2.into_inner() * proj;
                // NOTE: we set:
                // shift = world2 - world1
                let shift = -shift;

                if let Some((n, d)) = Unit::try_new_and_get(shift, na::zero()) {
                    world2 = world1 + shift;

                    if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
                        || s2.tangent_cone_contains_dir(
                            self.approx2.feature,
                            m2,
                            deformations2,
                            &-n,
                        )
                    {
                        depth = d;
                        normal = -n;
                    } else {
                        depth = -d;
                        normal = n;
                    }
                } else {
                    depth = na::zero();
                    normal = m1 * default_normal1;
                }
            }
            (NeighborhoodGeometry::Line(dir1), NeighborhoodGeometry::Line(dir2)) => {
                let world_dir1 = m1 * dir1;
                let world_dir2 = m2 * dir2;
                let (pt1, pt2) =
                    query::closest_points_line_line(&world1, &world_dir1, &world2, &world_dir2);

                world1 = pt1;
                world2 = pt2;

                if let Some((n, d)) = Unit::try_new_and_get(world2 - world1, na::zero()) {
                    if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
                        || s2.tangent_cone_contains_dir(
                            self.approx2.feature,
                            m2,
                            deformations2,
                            &-n,
                        )
                    {
                        depth = d;
                        normal = -n;
                    } else {
                        depth = -d;
                        normal = n;
                    }
                } else {
                    depth = na::zero();
                    normal = m1 * default_normal1;
                }
            }
            _ => {
                return None;
            }
        }

        world1 += normal.into_inner() * self.margin1;
        world2 += normal.into_inner() * (-self.margin2);
        depth += self.margin1 + self.margin2;

        Some(Contact::new(world1, world2, normal, depth))
    }
}