Simbody 3.7
Loading...
Searching...
No Matches
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
31namespace SimTK {
32
33//==============================================================================
34// CONTACT TRACKER
35//==============================================================================
63public:
64class HalfSpaceSphere;
66class HalfSpaceBrick;
69class SphereSphere;
74
77 ContactGeometryTypeId typeOfSurface2)
78: m_surfaceTypes(typeOfSurface1, typeOfSurface2)
79{
80}
81
84const std::pair<ContactGeometryTypeId,ContactGeometryTypeId>&
85getContactGeometryTypeIds() const {return m_surfaceTypes;}
86
87virtual ~ContactTracker() {}
88
94virtual 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
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
137 (const ContactGeometry& shapeA, const Vec3& pointP,
138 const ContactGeometry& shapeB, const Vec3& pointQ,
139 const Transform& X_AB);
140
146 (const ContactGeometry& shapeA, const Vec3& pointP,
147 const ContactGeometry& shapeB, const Vec3& pointQ,
148 const Transform& X_AB, const Vec6& err0);
149
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.
171std::pair<ContactGeometryTypeId,ContactGeometryTypeId> m_surfaceTypes;
172};
173
174
175
176//==============================================================================
177// HALFSPACE-SPHERE CONTACT TRACKER
178//==============================================================================
182: public ContactTracker {
183public:
185: ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
186 ContactGeometry::Sphere::classTypeId()) {}
187
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;
196};
197
198
199
200//==============================================================================
201// HALFSPACE-ELLIPSOID CONTACT TRACKER
202//==============================================================================
206: public ContactTracker {
207public:
209: ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
210 ContactGeometry::Ellipsoid::classTypeId()) {}
211
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;
220};
221
222
223
224//==============================================================================
225// HALFSPACE-BRICK CONTACT TRACKER
226//==============================================================================
230: public ContactTracker {
231public:
233: ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
234 ContactGeometry::Brick::classTypeId()) {}
235
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;
244};
245
246
247
248//==============================================================================
249// SPHERE-SPHERE CONTACT TRACKER
250//==============================================================================
254: public ContactTracker {
255public:
257: ContactTracker(ContactGeometry::Sphere::classTypeId(),
258 ContactGeometry::Sphere::classTypeId()) {}
259
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;
268};
269
270
271
272//==============================================================================
273// HALFSPACE-TRIANGLE MESH CONTACT TRACKER
274//==============================================================================
278: public ContactTracker {
279public:
281: ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
282 ContactGeometry::TriangleMesh::classTypeId()) {}
283
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;
292
293private:
294void processBox(const ContactGeometry::TriangleMesh& mesh,
296 const Transform& X_HM, const UnitVec3& hsNormal_M,
297 Real hsFaceHeight_M, std::set<int>& insideFaces) const;
298void 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 {
311public:
313: ContactTracker(ContactGeometry::Sphere::classTypeId(),
314 ContactGeometry::TriangleMesh::classTypeId()) {}
315
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;
324
325private:
326void 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 {
342public:
344: ContactTracker(ContactGeometry::TriangleMesh::classTypeId(),
345 ContactGeometry::TriangleMesh::classTypeId()) {}
346
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;
355
356private:
357void findIntersectingFaces
358 (const ContactGeometry::TriangleMesh& mesh1,
362 const OrientedBoundingBox& node2Bounds_M1,
363 const Transform& X_M1M2,
364 std::set<int>& insideFaces1,
365 std::set<int>& insideFaces2) const;
366
367void findBuriedFaces
369 const ContactGeometry::TriangleMesh& otherMesh,
370 const Transform& X_OM,
371 std::set<int>& insideFaces) const;
372
373void 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 {
392public:
394 (ContactGeometryTypeId typeOfConvexImplicitSurface)
395: ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
396 typeOfConvexImplicitSurface) {}
397
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;
406};
407
408
409//==============================================================================
410// CONVEX IMPLICIT SURFACE PAIR CONTACT TRACKER
411//==============================================================================
416: public ContactTracker {
417public:
420
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;
429};
430
431
432//==============================================================================
433// GENERAL IMPLICIT SURFACE PAIR CONTACT TRACKER
434//==============================================================================
443: public ContactTracker {
444public:
447
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;
456};
457
458} // namespace SimTK
459
460#endif // SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
#define SimTK_SIMMATH_EXPORT
Definition SimTKmath/include/simmath/internal/common.h:64
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition Array.h:1520
This is a unique integer type for quickly identifying specific types of contact geometry for fast loo...
This class represents a node in the Oriented Bounding Box Tree for a TriangleMesh.
Definition ContactGeometry.h:1343
This ContactGeometry subclass represents an arbitrary shape described by a mesh of triangular faces.
Definition ContactGeometry.h:1164
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 two smooth, convex objects by using their implicit funct...
Definition ContactTracker.h:416
ConvexImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2)
Definition ContactTracker.h:418
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
(TODO: not implemented yet) This ContactTracker handles contacts between two arbitrary smooth surface...
Definition ContactTracker.h:443
GeneralImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2)
Definition ContactTracker.h:445
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Sphe...
Definition ContactTracker.h:230
HalfSpaceBrick()
Definition ContactTracker.h:232
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and any ContactGeometry tha...
Definition ContactTracker.h:391
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
HalfSpaceConvexImplicit(ContactGeometryTypeId typeOfConvexImplicitSurface)
Definition ContactTracker.h:394
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Elli...
Definition ContactTracker.h:206
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
HalfSpaceEllipsoid()
Definition ContactTracker.h:208
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Sphe...
Definition ContactTracker.h:182
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
HalfSpaceSphere()
Definition ContactTracker.h:184
This ContactTracker handles contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Tria...
Definition ContactTracker.h:278
HalfSpaceTriangleMesh()
Definition ContactTracker.h:280
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
This ContactTracker handles contacts between two ContactGeometry::Sphere objects.
Definition ContactTracker.h:254
SphereSphere()
Definition ContactTracker.h:256
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
This ContactTracker handles contacts between a ContactGeometry::Sphere and a ContactGeometry::Triangl...
Definition ContactTracker.h:310
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
SphereTriangleMesh()
Definition ContactTracker.h:312
This ContactTracker handles contacts between two ContactGeometry::TriangleMesh surfaces.
Definition ContactTracker.h:341
bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const override
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
TriangleMeshTriangleMesh()
Definition ContactTracker.h:343
A ContactTracker implements an algorithm for detecting overlaps or potential overlaps between pairs o...
Definition ContactTracker.h:62
virtual ~ContactTracker()
Definition ContactTracker.h:87
static bool estimateConvexImplicitPairContactUsingMPR(const ContactGeometry &shapeA, const ContactGeometry &shapeB, const Transform &X_AB, Vec3 &pointP_A, Vec3 &pointQ_B, UnitVec3 &dirInA, int &numIterations)
Use Minkowski Portal Refinement (XenoCollide method by G.
static bool refineImplicitPair(const ContactGeometry &shapeA, Vec3 &pointP_A, const ContactGeometry &shapeB, Vec3 &pointQ_B, const Transform &X_AB, Real accuracyRequested, Real &accuracyAchieved, int &numIterations)
Given two shapes for which implicit functions are known, and a rough-guess contact point for each sha...
static Mat66 calcImplicitPairJacobian(const ContactGeometry &shapeA, const Vec3 &pointP, const ContactGeometry &shapeB, const Vec3 &pointQ, const Transform &X_AB, const Vec6 &err0)
Calculate the partial derivatives of the findImplicitPairError() error function with respect to the l...
static Vec6 findImplicitPairError(const ContactGeometry &shapeA, const Vec3 &pointP, const ContactGeometry &shapeB, const Vec3 &pointQ, const Transform &X_AB)
Calculate the error function described in refineImplicitPair().
ContactTracker(ContactGeometryTypeId typeOfSurface1, ContactGeometryTypeId typeOfSurface2)
Base class constructor for use by the concrete classes.
Definition ContactTracker.h:76
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
virtual bool trackContact(const Contact &priorStatus, const Transform &X_GS1, const ContactGeometry &surface1, const Transform &X_GS2, const ContactGeometry &surface2, Real cutoff, Contact &currentStatus) const =0
The ContactTrackerSubsystem will invoke this method for any pair of contact surfaces that is already ...
A Contact contains information about the spatial relationship between two surfaces that are near,...
Definition Contact.h:85
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition Mat.h:97
This class represents a rectangular box with arbitrary position and orientation.
Definition OrientedBoundingBox.h:42
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
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:606