Simbody  3.8
CollisionDetectionAlgorithm.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_COLLISION_DETECTION_ALGORITHM_H_
2 #define SimTK_SIMMATH_COLLISION_DETECTION_ALGORITHM_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) 2008-12 Stanford University and the Authors. *
13  * Authors: 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"
31 
32 #include <map>
33 
34 namespace SimTK {
35 
44 public:
45  class HalfSpaceSphere;
46  class SphereSphere;
47  class HalfSpaceEllipsoid;
49  class SphereTriangleMesh;
51  class ConvexConvex;
68  virtual void processObjects
69  (ContactSurfaceIndex index1, const ContactGeometry& object1,
70  const Transform& transform1,
71  ContactSurfaceIndex index2, const ContactGeometry& object2,
72  const Transform& transform2,
73  Array_<Contact>& contacts) const = 0;
85  ContactGeometryTypeId type2,
86  CollisionDetectionAlgorithm* algorithm);
98 private:
99  struct AlgorithmMap
100  : public std::map<std::pair<ContactGeometryTypeId, ContactGeometryTypeId>,
101  CollisionDetectionAlgorithm*>
102  {
103  ~AlgorithmMap();
104  };
105 
106  static AlgorithmMap algorithmMap;
107 };
108 
115 public:
116  virtual ~HalfSpaceSphere() {}
118  (ContactSurfaceIndex index1, const ContactGeometry& object1,
119  const Transform& transform1,
120  ContactSurfaceIndex index2, const ContactGeometry& object2,
121  const Transform& transform2,
122  Array_<Contact>& contacts) const override;
123 };
124 
131 public:
132  virtual ~HalfSpaceEllipsoid() {}
134  (ContactSurfaceIndex index1, const ContactGeometry& object1,
135  const Transform& transform1,
136  ContactSurfaceIndex index2, const ContactGeometry& object2,
137  const Transform& transform2,
138  Array_<Contact>& contacts) const override;
139 };
140 
146 public:
147  virtual ~SphereSphere() {}
149  (ContactSurfaceIndex index1, const ContactGeometry& object1,
150  const Transform& transform1,
151  ContactSurfaceIndex index2, const ContactGeometry& object2,
152  const Transform& transform2,
153  Array_<Contact>& contacts) const override;
154 };
155 
162 public:
165  (ContactSurfaceIndex index1, const ContactGeometry& object1,
166  const Transform& transform1,
167  ContactSurfaceIndex index2, const ContactGeometry& object2,
168  const Transform& transform2,
169  Array_<Contact>& contacts) const override;
170 private:
171  void processBox(const ContactGeometry::TriangleMesh& mesh,
173  const Transform& transform, const Vec3& axisDir,
174  Real xoffset, std::set<int>& insideFaces) const;
175  void addAllTriangles(const ContactGeometry::TriangleMesh::OBBTreeNode& node,
176  std::set<int>& insideFaces) const;
177 };
178 
185 public:
186  virtual ~SphereTriangleMesh() {}
188  (ContactSurfaceIndex index1, const ContactGeometry& object1,
189  const Transform& transform1,
190  ContactSurfaceIndex index2, const ContactGeometry& object2,
191  const Transform& transform2,
192  Array_<Contact>& contacts) const override;
193 private:
194  void processBox(const Vec3& center, Real radius2,
195  const ContactGeometry::TriangleMesh& mesh,
197  std::set<int>& insideFaces) const;
198 };
199 
206 public:
209  (ContactSurfaceIndex index1, const ContactGeometry& object1,
210  const Transform& transform1,
211  ContactSurfaceIndex index2, const ContactGeometry& object2,
212  const Transform& transform2,
213  Array_<Contact>& contacts) const override;
214 private:
215  void processNodes(const ContactGeometry::TriangleMesh& mesh1,
216  const ContactGeometry::TriangleMesh& mesh2,
219  const OrientedBoundingBox& node2Bounds,
220  const Transform& transform, std::set<int>& triangles1,
221  std::set<int>& triangles2) const;
222  void findInsideTriangles(const ContactGeometry::TriangleMesh& mesh,
223  const ContactGeometry::TriangleMesh& otherMesh,
224  const Transform& transform,
225  std::set<int>& triangles) const;
226  void tagFaces(const ContactGeometry::TriangleMesh& mesh,
227  Array_<int>& faceType, std::set<int>& triangles,
228  int index, int depth) const;
229  static const int OUTSIDE = -1;
230  static const int UNKNOWN = 0;
231  static const int BOUNDARY = 1;
232  static const int INSIDE = 2;
233 };
234 
240 public:
241  virtual ~ConvexConvex() {}
243  (ContactSurfaceIndex index1, const ContactGeometry& object1,
244  const Transform& transform1,
245  ContactSurfaceIndex index2, const ContactGeometry& object2,
246  const Transform& transform2,
247  Array_<Contact>& contacts) const override;
248 private:
249  static Vec3 computeSupport(const ContactGeometry& object1,
250  const ContactGeometry& object2,
251  const Transform& transform, UnitVec3 direction);
252  static void addContact
254  const ContactGeometry& object1,
255  const ContactGeometry& object2,
256  const Transform& transform1, const Transform& transform2,
257  const Transform& transform12,
258  Vec3 point1, Vec3 point2, Array_<Contact>& contacts);
259  static Vec6 computeErrorVector(const ContactGeometry& object1,
260  const ContactGeometry& object2,
261  Vec3 pos1, Vec3 pos2,
262  const Transform& transform12);
263  static Mat66 computeJacobian(const ContactGeometry& object1,
264  const ContactGeometry& object2,
265  Vec3 pos1, Vec3 pos2,
266  const Transform& transform12);
267 };
268 
269 } // namespace SimTK
270 
271 #endif // SimTK_SIMMATH_COLLISION_DETECTION_ALGORITHM_H_
Defines the ContactGeometry class and its API-visible local subclasses for individual contact shapes.
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 algorithm detects contacts between two ContactGeometry::Convex objects.
Definition: CollisionDetectionAlgorithm.h:239
void processObjects(ContactSurfaceIndex index1, const ContactGeometry &object1, const Transform &transform1, ContactSurfaceIndex index2, const ContactGeometry &object2, const Transform &transform2, Array_< Contact > &contacts) const override
Identify contacts between a pair of bodies.
virtual ~ConvexConvex()
Definition: CollisionDetectionAlgorithm.h:241
This algorithm detects contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Ellipsoid...
Definition: CollisionDetectionAlgorithm.h:130
virtual ~HalfSpaceEllipsoid()
Definition: CollisionDetectionAlgorithm.h:132
void processObjects(ContactSurfaceIndex index1, const ContactGeometry &object1, const Transform &transform1, ContactSurfaceIndex index2, const ContactGeometry &object2, const Transform &transform2, Array_< Contact > &contacts) const override
Identify contacts between a pair of bodies.
This algorithm detects contacts between a ContactGeometry::HalfSpace and a ContactGeometry::Sphere.
Definition: CollisionDetectionAlgorithm.h:114
virtual ~HalfSpaceSphere()
Definition: CollisionDetectionAlgorithm.h:116
void processObjects(ContactSurfaceIndex index1, const ContactGeometry &object1, const Transform &transform1, ContactSurfaceIndex index2, const ContactGeometry &object2, const Transform &transform2, Array_< Contact > &contacts) const override
Identify contacts between a pair of bodies.
This algorithm detects contacts between a ContactGeometry::HalfSpace and a ContactGeometry::TriangleM...
Definition: CollisionDetectionAlgorithm.h:161
void processObjects(ContactSurfaceIndex index1, const ContactGeometry &object1, const Transform &transform1, ContactSurfaceIndex index2, const ContactGeometry &object2, const Transform &transform2, Array_< Contact > &contacts) const override
Identify contacts between a pair of bodies.
virtual ~HalfSpaceTriangleMesh()
Definition: CollisionDetectionAlgorithm.h:163
This algorithm detects contacts between two ContactGeometry::Sphere objects.
Definition: CollisionDetectionAlgorithm.h:145
void processObjects(ContactSurfaceIndex index1, const ContactGeometry &object1, const Transform &transform1, ContactSurfaceIndex index2, const ContactGeometry &object2, const Transform &transform2, Array_< Contact > &contacts) const override
Identify contacts between a pair of bodies.
virtual ~SphereSphere()
Definition: CollisionDetectionAlgorithm.h:147
This algorithm detects contacts between a ContactGeometry::Sphere and a ContactGeometry::TriangleMesh...
Definition: CollisionDetectionAlgorithm.h:184
void processObjects(ContactSurfaceIndex index1, const ContactGeometry &object1, const Transform &transform1, ContactSurfaceIndex index2, const ContactGeometry &object2, const Transform &transform2, Array_< Contact > &contacts) const override
Identify contacts between a pair of bodies.
virtual ~SphereTriangleMesh()
Definition: CollisionDetectionAlgorithm.h:186
This algorithm detects contacts between two ContactGeometry::TriangleMesh objects.
Definition: CollisionDetectionAlgorithm.h:205
void processObjects(ContactSurfaceIndex index1, const ContactGeometry &object1, const Transform &transform1, ContactSurfaceIndex index2, const ContactGeometry &object2, const Transform &transform2, Array_< Contact > &contacts) const override
Identify contacts between a pair of bodies.
virtual ~TriangleMeshTriangleMesh()
Definition: CollisionDetectionAlgorithm.h:207
A CollisionDetectionAlgorithm implements an algorithm for detecting overlaps between pairs of Contact...
Definition: CollisionDetectionAlgorithm.h:43
static CollisionDetectionAlgorithm * getAlgorithm(ContactGeometryTypeId type1, ContactGeometryTypeId type2)
Get the CollisionDetectionAlgorithm to use for identifying contacts between bodies of two specific ty...
virtual void processObjects(ContactSurfaceIndex index1, const ContactGeometry &object1, const Transform &transform1, ContactSurfaceIndex index2, const ContactGeometry &object2, const Transform &transform2, Array_< Contact > &contacts) const =0
Identify contacts between a pair of bodies.
virtual ~CollisionDetectionAlgorithm()
Definition: CollisionDetectionAlgorithm.h:52
static void registerAlgorithm(ContactGeometryTypeId type1, ContactGeometryTypeId type2, CollisionDetectionAlgorithm *algorithm)
Register a CollisionDetectionAlgorithm to be used for identifying contacts between bodies of two spec...
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:1495
This ContactGeometry subclass represents an arbitrary shape described by a mesh of triangular faces.
Definition: ContactGeometry.h:1316
A ContactGeometry object describes the shape of all or part of the boundary of a solid object,...
Definition: ContactGeometry.h:111
This defines a unique index for all the contact surfaces being handled either by a ContactTrackerSubs...
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:607