Simbody  3.5
ContactTracker.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
2 #define SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKmath *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2010-14 Stanford University and the Authors. *
13  * Authors: Michael Sherman, Peter Eastman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKcommon.h"
30 
31 namespace SimTK {
32 
33 //==============================================================================
34 // CONTACT TRACKER
35 //==============================================================================
63 public:
64 class HalfSpaceSphere;
65 class HalfSpaceEllipsoid;
66 class HalfSpaceBrick;
69 class SphereSphere;
70 class SphereTriangleMesh;
72 class ConvexImplicitPair;
74 
77  ContactGeometryTypeId typeOfSurface2)
78 : m_surfaceTypes(typeOfSurface1, typeOfSurface2)
79 {
80 }
81 
84 const std::pair<ContactGeometryTypeId,ContactGeometryTypeId>&
85 getContactGeometryTypeIds() const {return m_surfaceTypes;}
86 
87 virtual ~ContactTracker() {}
88 
94 virtual bool trackContact
95  (const Contact& priorStatus,
96  const Transform& X_GS1,
97  const ContactGeometry& surface1,
98  const Transform& X_GS2,
99  const ContactGeometry& surface2,
100  Real cutoff,
101  Contact& currentStatus) const = 0;
102 
129 static bool refineImplicitPair
130  (const ContactGeometry& shapeA, Vec3& pointP_A, // in/out
131  const ContactGeometry& shapeB, Vec3& pointQ_B, // in/out
132  const Transform& X_AB, Real accuracyRequested,
133  Real& accuracyAchieved, int& numIterations);
134 
136 static Vec6 findImplicitPairError
137  (const ContactGeometry& shapeA, const Vec3& pointP,
138  const ContactGeometry& shapeB, const Vec3& pointQ,
139  const Transform& X_AB);
140 
145 static Mat66 calcImplicitPairJacobian
146  (const ContactGeometry& shapeA, const Vec3& pointP,
147  const ContactGeometry& shapeB, const Vec3& pointQ,
148  const Transform& X_AB, const Vec6& err0);
149 
160 static bool estimateConvexImplicitPairContactUsingMPR
161  (const ContactGeometry& shapeA, const ContactGeometry& shapeB,
162  const Transform& X_AB,
163  Vec3& pointP_A, Vec3& pointQ_B, UnitVec3& dirInA,
164  int& numIterations);
165 
166 
167 //--------------------------------------------------------------------------
168  private:
169 // This tracker should be called only for surfaces of these two types,
170 // in this order.
171 std::pair<ContactGeometryTypeId,ContactGeometryTypeId> m_surfaceTypes;
172 };
173 
174 
175 
176 //==============================================================================
177 // HALFSPACE-SPHERE CONTACT TRACKER
178 //==============================================================================
182 : public ContactTracker {
183 public:
185 : ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
186  ContactGeometry::Sphere::classTypeId()) {}
187 
188 bool trackContact
189  (const Contact& priorStatus,
190  const Transform& X_GS1,
191  const ContactGeometry& surface1,
192  const Transform& X_GS2,
193  const ContactGeometry& surface2,
194  Real cutoff,
195  Contact& currentStatus) const OVERRIDE_11;
196 };
197 
198 
199 
200 //==============================================================================
201 // HALFSPACE-ELLIPSOID CONTACT TRACKER
202 //==============================================================================
206 : public ContactTracker {
207 public:
209 : ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
210  ContactGeometry::Ellipsoid::classTypeId()) {}
211 
212 bool trackContact
213  (const Contact& priorStatus,
214  const Transform& X_GS1,
215  const ContactGeometry& surface1,
216  const Transform& X_GS2,
217  const ContactGeometry& surface2,
218  Real cutoff,
219  Contact& currentStatus) const OVERRIDE_11;
220 };
221 
222 
223 
224 //==============================================================================
225 // HALFSPACE-BRICK CONTACT TRACKER
226 //==============================================================================
230 : public ContactTracker {
231 public:
233 : ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
234  ContactGeometry::Brick::classTypeId()) {}
235 
236 bool trackContact
237  (const Contact& priorStatus,
238  const Transform& X_GS1,
239  const ContactGeometry& surface1,
240  const Transform& X_GS2,
241  const ContactGeometry& surface2,
242  Real cutoff,
243  Contact& currentStatus) const OVERRIDE_11;
244 };
245 
246 
247 
248 //==============================================================================
249 // SPHERE-SPHERE CONTACT TRACKER
250 //==============================================================================
254 : public ContactTracker {
255 public:
257 : ContactTracker(ContactGeometry::Sphere::classTypeId(),
258  ContactGeometry::Sphere::classTypeId()) {}
259 
260 bool trackContact
261  (const Contact& priorStatus,
262  const Transform& X_GS1,
263  const ContactGeometry& surface1,
264  const Transform& X_GS2,
265  const ContactGeometry& surface2,
266  Real cutoff,
267  Contact& currentStatus) const OVERRIDE_11;
268 };
269 
270 
271 
272 //==============================================================================
273 // HALFSPACE-TRIANGLE MESH CONTACT TRACKER
274 //==============================================================================
278 : public ContactTracker {
279 public:
281 : ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
282  ContactGeometry::TriangleMesh::classTypeId()) {}
283 
284 bool trackContact
285  (const Contact& priorStatus,
286  const Transform& X_GS1,
287  const ContactGeometry& surface1, // the half space
288  const Transform& X_GS2,
289  const ContactGeometry& surface2, // the mesh
290  Real cutoff,
291  Contact& currentStatus) const OVERRIDE_11;
292 
293 private:
294 void processBox(const ContactGeometry::TriangleMesh& mesh,
296  const Transform& X_HM, const UnitVec3& hsNormal_M,
297  Real hsFaceHeight_M, std::set<int>& insideFaces) const;
298 void addAllTriangles(const ContactGeometry::TriangleMesh::OBBTreeNode& node,
299  std::set<int>& insideFaces) const;
300 };
301 
302 
303 
304 //==============================================================================
305 // SPHERE - TRIANGLE MESH CONTACT TRACKER
306 //==============================================================================
310 : public ContactTracker {
311 public:
313 : ContactTracker(ContactGeometry::Sphere::classTypeId(),
314  ContactGeometry::TriangleMesh::classTypeId()) {}
315 
316 bool trackContact
317  (const Contact& priorStatus,
318  const Transform& X_GS1,
319  const ContactGeometry& surface1, // the sphere
320  const Transform& X_GS2,
321  const ContactGeometry& surface2, // the mesh
322  Real cutoff,
323  Contact& currentStatus) const OVERRIDE_11;
324 
325 private:
326 void processBox
327  (const ContactGeometry::TriangleMesh& mesh,
329  const Vec3& center_M, Real radius2,
330  std::set<int>& insideFaces) const ;
331 };
332 
333 
334 
335 //==============================================================================
336 // TRIANGLE MESH - TRIANGLE MESH CONTACT TRACKER
337 //==============================================================================
341 : public ContactTracker {
342 public:
344 : ContactTracker(ContactGeometry::TriangleMesh::classTypeId(),
345  ContactGeometry::TriangleMesh::classTypeId()) {}
346 
347 bool trackContact
348  (const Contact& priorStatus,
349  const Transform& X_GS1,
350  const ContactGeometry& surface1, // mesh1
351  const Transform& X_GS2,
352  const ContactGeometry& surface2, // mesh2
353  Real cutoff,
354  Contact& currentStatus) const OVERRIDE_11;
355 
356 private:
357 void findIntersectingFaces
358  (const ContactGeometry::TriangleMesh& mesh1,
359  const ContactGeometry::TriangleMesh& mesh2,
362  const OrientedBoundingBox& node2Bounds_M1,
363  const Transform& X_M1M2,
364  std::set<int>& insideFaces1,
365  std::set<int>& insideFaces2) const;
366 
367 void findBuriedFaces
368  (const ContactGeometry::TriangleMesh& mesh,
369  const ContactGeometry::TriangleMesh& otherMesh,
370  const Transform& X_OM,
371  std::set<int>& insideFaces) const;
372 
373 void tagFaces(const ContactGeometry::TriangleMesh& mesh,
374  Array_<int>& faceType,
375  std::set<int>& triangles,
376  int index,
377  int depth) const;
378 };
379 
380 
381 //==============================================================================
382 // HALFSPACE-CONVEX IMPLICIT CONTACT TRACKER
383 //==============================================================================
391 : public ContactTracker {
392 public:
394  (ContactGeometryTypeId typeOfConvexImplicitSurface)
396  typeOfConvexImplicitSurface) {}
397 
398 bool trackContact
399  (const Contact& priorStatus,
400  const Transform& X_GS1,
401  const ContactGeometry& surface1, // the half-space
402  const Transform& X_GS2,
403  const ContactGeometry& surface2, // the convex implicit surface
404  Real cutoff,
405  Contact& currentStatus) const OVERRIDE_11;
406 };
407 
408 
409 //==============================================================================
410 // CONVEX IMPLICIT SURFACE PAIR CONTACT TRACKER
411 //==============================================================================
416 : public ContactTracker {
417 public:
419 : ContactTracker(type1, type2) {}
420 
421 bool trackContact
422  (const Contact& priorStatus,
423  const Transform& X_GS1,
424  const ContactGeometry& surface1,
425  const Transform& X_GS2,
426  const ContactGeometry& surface2,
427  Real cutoff,
428  Contact& currentStatus) const OVERRIDE_11;
429 };
430 
431 
432 //==============================================================================
433 // GENERAL IMPLICIT SURFACE PAIR CONTACT TRACKER
434 //==============================================================================
443 : public ContactTracker {
444 public:
446 : ContactTracker(type1, type2) {}
447 
448 bool trackContact
449  (const Contact& priorStatus,
450  const Transform& X_GS1,
451  const ContactGeometry& surface1,
452  const Transform& X_GS2,
453  const ContactGeometry& surface2,
454  Real cutoff,
455  Contact& currentStatus) const OVERRIDE_11;
456 };
457 
458 } // namespace SimTK
459 
460 #endif // SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
SphereTriangleMesh()
Definition: ContactTracker.h:312
HalfSpaceSphere()
Definition: ContactTracker.h:184
This ContactTracker handles contacts between two smooth, convex objects by using their implicit funct...
Definition: ContactTracker.h:415
GeneralImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2)
Definition: ContactTracker.h:445
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and any ContactGeometry tha...
Definition: ContactTracker.h:390
This is a unique integer type for quickly identifying specific types of contact geometry for fast loo...
HalfSpaceBrick()
Definition: ContactTracker.h:232
SphereSphere()
Definition: ContactTracker.h:256
A ContactTracker implements an algorithm for detecting overlaps or potential overlaps between pairs o...
Definition: ContactTracker.h:62
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Tria...
Definition: ContactTracker.h:277
ContactTracker(ContactGeometryTypeId typeOfSurface1, ContactGeometryTypeId typeOfSurface2)
Base class constructor for use by the concrete classes.
Definition: ContactTracker.h:76
HalfSpaceTriangleMesh()
Definition: ContactTracker.h:280
virtual ~ContactTracker()
Definition: ContactTracker.h:87
This class represents a rectangular box with arbitrary position and orientation.
Definition: OrientedBoundingBox.h:42
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:593
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Sphe...
Definition: ContactTracker.h:229
This class represents a node in the Oriented Bounding Box Tree for a TriangleMesh.
Definition: ContactGeometry.h:1343
(TODO: not implemented yet) This ContactTracker handles contacts between two arbitrary smooth surface...
Definition: ContactTracker.h:442
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
ConvexImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2)
Definition: ContactTracker.h:418
#define OVERRIDE_11
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:267
This ContactGeometry subclass represents an arbitrary shape described by a mesh of triangular faces...
Definition: ContactGeometry.h:1163
const std::pair< ContactGeometryTypeId, ContactGeometryTypeId > & getContactGeometryTypeIds() const
Return the pair of contact geometry type ids handled by this tracker, in the order that they must be ...
Definition: ContactTracker.h:85
HalfSpaceEllipsoid()
Definition: ContactTracker.h:208
A ContactGeometry object describes the shape of all or part of the boundary of a solid object...
Definition: ContactGeometry.h:110
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Elli...
Definition: ContactTracker.h:205
This ContactTracker handles contacts between a ContactGeometry::Sphere and a ContactGeometry::Triangl...
Definition: ContactTracker.h:309
TriangleMeshTriangleMesh()
Definition: ContactTracker.h:343
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Sphe...
Definition: ContactTracker.h:181
This is the header file that every Simmath compilation unit should include first. ...
A Contact contains information about the spatial relationship between two surfaces that are near...
Definition: Contact.h:83
This ContactTracker handles contacts between two ContactGeometry::TriangleMesh surfaces.
Definition: ContactTracker.h:340
static ContactGeometryTypeId classTypeId()
Obtain the unique id for HalfSpace contact geometry.
This ContactTracker handles contacts between two ContactGeometry::Sphere objects. ...
Definition: ContactTracker.h:253
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64