ncollide3d/query/contact/
contact_kinematic.rs

1use crate::math::{Isometry, Point, Vector};
2use crate::query::{self, Contact};
3use crate::shape::{FeatureId, Shape};
4use na::{self, RealField, Unit};
5
6/// 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.
10    Point,
11    /// A line approximation.
12    Line(Unit<Vector<N>>),
13    /// A planar approximation.
14    Plane(Unit<Vector<N>>),
15}
16
17/// 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.
32    pub feature: FeatureId,
33    /// The point where approximation is computed.
34    pub point: Point<N>,
35    /// The approximation geometry.
36    pub geometry: NeighborhoodGeometry<N>,
37}
38
39impl<N: RealField + Copy> LocalShapeApproximation<N> {
40    /// Initializes a new local shape approximation at `point`.
41    pub fn new(feature: FeatureId, point: Point<N>, geometry: NeighborhoodGeometry<N>) -> Self {
42        LocalShapeApproximation {
43            feature,
44            point,
45            geometry,
46        }
47    }
48}
49
50/// 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>,
60
61    margin1: N,
62    margin2: N,
63}
64
65impl<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_`.
70    pub fn new() -> Self {
71        let approx = LocalShapeApproximation::new(
72            FeatureId::Unknown,
73            Point::origin(),
74            NeighborhoodGeometry::Point,
75        );
76
77        ContactKinematic {
78            margin1: na::zero(),
79            margin2: na::zero(),
80            approx1: approx.clone(),
81            approx2: approx,
82        }
83    }
84
85    /// Applies the given transformation to the first set of contact information.
86    pub fn transform1(&mut self, m: &Isometry<N>) {
87        self.approx1.point = m * self.approx1.point;
88
89        match &mut self.approx1.geometry {
90            NeighborhoodGeometry::Point => {}
91            NeighborhoodGeometry::Plane(n) | NeighborhoodGeometry::Line(n) => {
92                *n = m * &*n;
93            }
94        }
95    }
96
97    /// Applies the given transformation to the second set of contact information.
98    pub fn transform2(&mut self, m: &Isometry<N>) {
99        self.approx2.point = m * self.approx2.point;
100
101        match &mut self.approx2.geometry {
102            NeighborhoodGeometry::Point => {}
103            NeighborhoodGeometry::Plane(n) | NeighborhoodGeometry::Line(n) => {
104                *n = m * &*n;
105            }
106        }
107    }
108
109    /// The dilation of the first solid.
110    pub fn dilation1(&self) -> N {
111        self.margin1
112    }
113
114    /// The dilation of the second solid.
115    pub fn dilation2(&self) -> N {
116        self.margin2
117    }
118
119    /// 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.
125    pub fn local1(&self) -> Point<N> {
126        self.approx1.point
127    }
128
129    /// 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.
135    pub fn local2(&self) -> Point<N> {
136        self.approx2.point
137    }
138
139    /// The shape-dependent identifier of the feature of the first solid
140    /// on which lies the contact point.
141    pub fn feature1(&self) -> FeatureId {
142        self.approx1.feature
143    }
144
145    /// The shape-dependent identifier of the feature of the second solid
146    /// on which lies the contact point.
147    pub fn feature2(&self) -> FeatureId {
148        self.approx2.feature
149    }
150
151    /// Sets the shape-dependent identifier of the feature of the first solid
152    /// on which lies the contact point.
153    pub fn set_feature1(&mut self, f: FeatureId) {
154        self.approx1.feature = f
155    }
156
157    /// Sets the shape-dependent identifier of the feature of the second solid
158    /// on which lies the contact point.
159    pub fn set_feature2(&mut self, f: FeatureId) {
160        self.approx2.feature = f
161    }
162
163    /// Sets the dilation of the first solid.
164    pub fn set_dilation1(&mut self, margin: N) {
165        self.margin1 = margin;
166    }
167
168    /// Sets the dilation of the second solid.
169    pub fn set_dilation2(&mut self, margin: N) {
170        self.margin2 = margin;
171    }
172
173    /// The local approximation of the first shape.
174    pub fn approx1(&self) -> &LocalShapeApproximation<N> {
175        &self.approx1
176    }
177
178    /// The local approximation of the first shape.
179    pub fn approx2(&self) -> &LocalShapeApproximation<N> {
180        &self.approx2
181    }
182
183    /// The local approximation of the first shape.
184    pub fn approx1_mut(&mut self) -> &mut LocalShapeApproximation<N> {
185        &mut self.approx1
186    }
187
188    /// The local approximation of the second shape.
189    pub fn approx2_mut(&mut self) -> &mut LocalShapeApproximation<N> {
190        &mut self.approx2
191    }
192
193    /// Sets the local approximation of the first shape.
194    pub fn set_approx1(
195        &mut self,
196        feature: FeatureId,
197        point: Point<N>,
198        geom: NeighborhoodGeometry<N>,
199    ) {
200        self.approx1 = LocalShapeApproximation::new(feature, point, geom);
201    }
202
203    /// Sets the local approximation of the second shape.
204    pub fn set_approx2(
205        &mut self,
206        feature: FeatureId,
207        point: Point<N>,
208        geom: NeighborhoodGeometry<N>,
209    ) {
210        self.approx2 = LocalShapeApproximation::new(feature, point, geom);
211    }
212
213    /// 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.
218    pub 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>> {
228        let normal;
229        let mut depth;
230
231        let mut world1 = m1 * self.approx1.point;
232        let mut world2 = m2 * self.approx2.point;
233
234        match (&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)) => {
241                let 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) => {
247                if let Some((n, d)) = Unit::try_new_and_get(world2 - world1, N::default_epsilon()) {
248                    if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
249                        || s2.tangent_cone_contains_dir(
250                            self.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) => {
268                let world_dir1 = m1 * dir1;
269                let mut shift = world2 - world1;
270                let proj = world_dir1.dot(&shift);
271                shift -= dir1.into_inner() * proj;
272
273                if let Some((n, d)) = Unit::try_new_and_get(shift, na::zero()) {
274                    world1 = world2 + (-shift);
275
276                    if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
277                        || s2.tangent_cone_contains_dir(
278                            self.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)) => {
296                let world_dir2 = m2 * dir2;
297                let mut shift = world1 - world2;
298                let proj = world_dir2.dot(&shift);
299                shift -= dir2.into_inner() * proj;
300                // NOTE: we set:
301                // shift = world2 - world1
302                let shift = -shift;
303
304                if let Some((n, d)) = Unit::try_new_and_get(shift, na::zero()) {
305                    world2 = world1 + shift;
306
307                    if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
308                        || s2.tangent_cone_contains_dir(
309                            self.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)) => {
327                let world_dir1 = m1 * dir1;
328                let world_dir2 = m2 * dir2;
329                let (pt1, pt2) =
330                    query::closest_points_line_line(&world1, &world_dir1, &world2, &world_dir2);
331
332                world1 = pt1;
333                world2 = pt2;
334
335                if let Some((n, d)) = Unit::try_new_and_get(world2 - world1, na::zero()) {
336                    if s1.tangent_cone_contains_dir(self.approx1.feature, m1, deformations1, &n)
337                        || s2.tangent_cone_contains_dir(
338                            self.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            _ => {
356                return None;
357            }
358        }
359
360        world1 += normal.into_inner() * self.margin1;
361        world2 += normal.into_inner() * (-self.margin2);
362        depth += self.margin1 + self.margin2;
363
364        Some(Contact::new(world1, world2, normal, depth))
365    }
366}