Bullet Collision Detection & Physics Library
btMinkowskiPenetrationDepthSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
21 
22 #define NUM_UNITSPHERE_POINTS 42
23 
25  const btConvexShape* convexA, const btConvexShape* convexB,
26  const btTransform& transA, const btTransform& transB,
27  btVector3& v, btVector3& pa, btVector3& pb,
28  class btIDebugDraw* debugDraw)
29 {
30  (void)v;
31 
32  bool check2d = convexA->isConvex2d() && convexB->isConvex2d();
33 
34  struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
35  {
36  btIntermediateResult() : m_hasResult(false)
37  {
38  }
39 
40  btVector3 m_normalOnBInWorld;
41  btVector3 m_pointInWorld;
42  btScalar m_depth;
43  bool m_hasResult;
44 
45  virtual void setShapeIdentifiersA(int partId0, int index0)
46  {
47  (void)partId0;
48  (void)index0;
49  }
50  virtual void setShapeIdentifiersB(int partId1, int index1)
51  {
52  (void)partId1;
53  (void)index1;
54  }
55  void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
56  {
57  m_normalOnBInWorld = normalOnBInWorld;
58  m_pointInWorld = pointInWorld;
59  m_depth = depth;
60  m_hasResult = true;
61  }
62  };
63 
64  //just take fixed number of orientation, and sample the penetration depth in that direction
65  btScalar minProj = btScalar(BT_LARGE_FLOAT);
66  btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
67  btVector3 minA, minB;
68  btVector3 seperatingAxisInA, seperatingAxisInB;
69  btVector3 pInA, qInB, pWorld, qWorld, w;
70 
71 #ifndef __SPU__
72 #define USE_BATCHED_SUPPORT 1
73 #endif
74 #ifdef USE_BATCHED_SUPPORT
75 
80  int i;
81 
82  int numSampleDirections = NUM_UNITSPHERE_POINTS;
83 
84  for (i = 0; i < numSampleDirections; i++)
85  {
87  seperatingAxisInABatch[i] = (-norm) * transA.getBasis();
88  seperatingAxisInBBatch[i] = norm * transB.getBasis();
89  }
90 
91  {
92  int numPDA = convexA->getNumPreferredPenetrationDirections();
93  if (numPDA)
94  {
95  for (int i = 0; i < numPDA; i++)
96  {
97  btVector3 norm;
98  convexA->getPreferredPenetrationDirection(i, norm);
99  norm = transA.getBasis() * norm;
100  getPenetrationDirections()[numSampleDirections] = norm;
101  seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
102  seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
103  numSampleDirections++;
104  }
105  }
106  }
107 
108  {
109  int numPDB = convexB->getNumPreferredPenetrationDirections();
110  if (numPDB)
111  {
112  for (int i = 0; i < numPDB; i++)
113  {
114  btVector3 norm;
115  convexB->getPreferredPenetrationDirection(i, norm);
116  norm = transB.getBasis() * norm;
117  getPenetrationDirections()[numSampleDirections] = norm;
118  seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
119  seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
120  numSampleDirections++;
121  }
122  }
123  }
124 
125  convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
126  convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
127 
128  for (i = 0; i < numSampleDirections; i++)
129  {
131  if (check2d)
132  {
133  norm[2] = 0.f;
134  }
135  if (norm.length2() > 0.01)
136  {
137  seperatingAxisInA = seperatingAxisInABatch[i];
138  seperatingAxisInB = seperatingAxisInBBatch[i];
139 
140  pInA = supportVerticesABatch[i];
141  qInB = supportVerticesBBatch[i];
142 
143  pWorld = transA(pInA);
144  qWorld = transB(qInB);
145  if (check2d)
146  {
147  pWorld[2] = 0.f;
148  qWorld[2] = 0.f;
149  }
150 
151  w = qWorld - pWorld;
152  btScalar delta = norm.dot(w);
153  //find smallest delta
154  if (delta < minProj)
155  {
156  minProj = delta;
157  minNorm = norm;
158  minA = pWorld;
159  minB = qWorld;
160  }
161  }
162  }
163 #else
164 
165  int numSampleDirections = NUM_UNITSPHERE_POINTS;
166 
167 #ifndef __SPU__
168  {
169  int numPDA = convexA->getNumPreferredPenetrationDirections();
170  if (numPDA)
171  {
172  for (int i = 0; i < numPDA; i++)
173  {
174  btVector3 norm;
175  convexA->getPreferredPenetrationDirection(i, norm);
176  norm = transA.getBasis() * norm;
177  getPenetrationDirections()[numSampleDirections] = norm;
178  numSampleDirections++;
179  }
180  }
181  }
182 
183  {
184  int numPDB = convexB->getNumPreferredPenetrationDirections();
185  if (numPDB)
186  {
187  for (int i = 0; i < numPDB; i++)
188  {
189  btVector3 norm;
190  convexB->getPreferredPenetrationDirection(i, norm);
191  norm = transB.getBasis() * norm;
192  getPenetrationDirections()[numSampleDirections] = norm;
193  numSampleDirections++;
194  }
195  }
196  }
197 #endif // __SPU__
198 
199  for (int i = 0; i < numSampleDirections; i++)
200  {
201  const btVector3& norm = getPenetrationDirections()[i];
202  seperatingAxisInA = (-norm) * transA.getBasis();
203  seperatingAxisInB = norm * transB.getBasis();
204  pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
205  qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
206  pWorld = transA(pInA);
207  qWorld = transB(qInB);
208  w = qWorld - pWorld;
209  btScalar delta = norm.dot(w);
210  //find smallest delta
211  if (delta < minProj)
212  {
213  minProj = delta;
214  minNorm = norm;
215  minA = pWorld;
216  minB = qWorld;
217  }
218  }
219 #endif //USE_BATCHED_SUPPORT
220 
221  //add the margins
222 
223  minA += minNorm * convexA->getMarginNonVirtual();
224  minB -= minNorm * convexB->getMarginNonVirtual();
225  //no penetration
226  if (minProj < btScalar(0.))
227  return false;
228 
229  btScalar extraSeparation = 0.5f;
230  minProj += extraSeparation + (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
231 
232 //#define DEBUG_DRAW 1
233 #ifdef DEBUG_DRAW
234  if (debugDraw)
235  {
236  btVector3 color(0, 1, 0);
237  debugDraw->drawLine(minA, minB, color);
238  color = btVector3(1, 1, 1);
239  btVector3 vec = minB - minA;
240  btScalar prj2 = minNorm.dot(vec);
241  debugDraw->drawLine(minA, minA + (minNorm * minProj), color);
242  }
243 #endif //DEBUG_DRAW
244 
245  btGjkPairDetector gjkdet(convexA, convexB, &simplexSolver, 0);
246 
247  btScalar offsetDist = minProj;
248  btVector3 offset = minNorm * offsetDist;
249 
251 
252  btVector3 newOrg = transA.getOrigin() + offset;
253 
254  btTransform displacedTrans = transA;
255  displacedTrans.setOrigin(newOrg);
256 
257  input.m_transformA = displacedTrans;
258  input.m_transformB = transB;
260 
261  btIntermediateResult res;
262  gjkdet.setCachedSeperatingAxis(-minNorm);
263  gjkdet.getClosestPoints(input, res, debugDraw);
264 
265  btScalar correctedMinNorm = minProj - res.m_depth;
266 
267  //the penetration depth is over-estimated, relax it
268  btScalar penetration_relaxation = btScalar(1.);
269  minNorm *= penetration_relaxation;
270 
271  if (res.m_hasResult)
272  {
273  pa = res.m_pointInWorld - minNorm * correctedMinNorm;
274  pb = res.m_pointInWorld;
275  v = minNorm;
276 
277 #ifdef DEBUG_DRAW
278  if (debugDraw)
279  {
280  btVector3 color(1, 0, 0);
281  debugDraw->drawLine(pa, pb, color);
282  }
283 #endif //DEBUG_DRAW
284  }
285  return res.m_hasResult;
286 }
287 
289 {
290  static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
291  {
292  btVector3(btScalar(0.000000), btScalar(-0.000000), btScalar(-1.000000)),
293  btVector3(btScalar(0.723608), btScalar(-0.525725), btScalar(-0.447219)),
294  btVector3(btScalar(-0.276388), btScalar(-0.850649), btScalar(-0.447219)),
295  btVector3(btScalar(-0.894426), btScalar(-0.000000), btScalar(-0.447216)),
296  btVector3(btScalar(-0.276388), btScalar(0.850649), btScalar(-0.447220)),
297  btVector3(btScalar(0.723608), btScalar(0.525725), btScalar(-0.447219)),
298  btVector3(btScalar(0.276388), btScalar(-0.850649), btScalar(0.447220)),
299  btVector3(btScalar(-0.723608), btScalar(-0.525725), btScalar(0.447219)),
300  btVector3(btScalar(-0.723608), btScalar(0.525725), btScalar(0.447219)),
301  btVector3(btScalar(0.276388), btScalar(0.850649), btScalar(0.447219)),
302  btVector3(btScalar(0.894426), btScalar(0.000000), btScalar(0.447216)),
303  btVector3(btScalar(-0.000000), btScalar(0.000000), btScalar(1.000000)),
304  btVector3(btScalar(0.425323), btScalar(-0.309011), btScalar(-0.850654)),
305  btVector3(btScalar(-0.162456), btScalar(-0.499995), btScalar(-0.850654)),
306  btVector3(btScalar(0.262869), btScalar(-0.809012), btScalar(-0.525738)),
307  btVector3(btScalar(0.425323), btScalar(0.309011), btScalar(-0.850654)),
308  btVector3(btScalar(0.850648), btScalar(-0.000000), btScalar(-0.525736)),
309  btVector3(btScalar(-0.525730), btScalar(-0.000000), btScalar(-0.850652)),
310  btVector3(btScalar(-0.688190), btScalar(-0.499997), btScalar(-0.525736)),
311  btVector3(btScalar(-0.162456), btScalar(0.499995), btScalar(-0.850654)),
312  btVector3(btScalar(-0.688190), btScalar(0.499997), btScalar(-0.525736)),
313  btVector3(btScalar(0.262869), btScalar(0.809012), btScalar(-0.525738)),
314  btVector3(btScalar(0.951058), btScalar(0.309013), btScalar(0.000000)),
315  btVector3(btScalar(0.951058), btScalar(-0.309013), btScalar(0.000000)),
316  btVector3(btScalar(0.587786), btScalar(-0.809017), btScalar(0.000000)),
317  btVector3(btScalar(0.000000), btScalar(-1.000000), btScalar(0.000000)),
318  btVector3(btScalar(-0.587786), btScalar(-0.809017), btScalar(0.000000)),
319  btVector3(btScalar(-0.951058), btScalar(-0.309013), btScalar(-0.000000)),
320  btVector3(btScalar(-0.951058), btScalar(0.309013), btScalar(-0.000000)),
321  btVector3(btScalar(-0.587786), btScalar(0.809017), btScalar(-0.000000)),
322  btVector3(btScalar(-0.000000), btScalar(1.000000), btScalar(-0.000000)),
323  btVector3(btScalar(0.587786), btScalar(0.809017), btScalar(-0.000000)),
324  btVector3(btScalar(0.688190), btScalar(-0.499997), btScalar(0.525736)),
325  btVector3(btScalar(-0.262869), btScalar(-0.809012), btScalar(0.525738)),
326  btVector3(btScalar(-0.850648), btScalar(0.000000), btScalar(0.525736)),
327  btVector3(btScalar(-0.262869), btScalar(0.809012), btScalar(0.525738)),
328  btVector3(btScalar(0.688190), btScalar(0.499997), btScalar(0.525736)),
329  btVector3(btScalar(0.525730), btScalar(0.000000), btScalar(0.850652)),
330  btVector3(btScalar(0.162456), btScalar(-0.499995), btScalar(0.850654)),
331  btVector3(btScalar(-0.425323), btScalar(-0.309011), btScalar(0.850654)),
332  btVector3(btScalar(-0.425323), btScalar(0.309011), btScalar(0.850654)),
333  btVector3(btScalar(0.162456), btScalar(0.499995), btScalar(0.850654))};
334 
335  return sPenetrationDirections;
336 }
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
virtual bool calcPenDepth(btSimplexSolverInterface &simplexSolver, const btConvexShape *convexA, const btConvexShape *convexB, const btTransform &transA, const btTransform &transB, btVector3 &v, btVector3 &pa, btVector3 &pb, class btIDebugDraw *debugDraw)
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual void getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const =0
bool isConvex2d() const
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
#define btSimplexSolverInterface
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
virtual int getNumPreferredPenetrationDirections() const =0
#define MAX_PREFERRED_PENETRATION_DIRECTIONS
Definition: btConvexShape.h:27
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:26
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const =0
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btScalar getMarginNonVirtual() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
void setCachedSeperatingAxis(const btVector3 &seperatingAxis)
#define NUM_UNITSPHERE_POINTS
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)