Bullet Collision Detection & Physics Library
btGjkPairDetector.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 
16 #include "btGjkPairDetector.h"
20 
21 
22 
23 #if defined(DEBUG) || defined (_DEBUG)
24 //#define TEST_NON_VIRTUAL 1
25 #include <stdio.h> //for debug printf
26 #ifdef __SPU__
27 #include <spu_printf.h>
28 #define printf spu_printf
29 #endif //__SPU__
30 #endif
31 
32 //must be above the machine epsilon
33 #ifdef BT_USE_DOUBLE_PRECISION
34  #define REL_ERROR2 btScalar(1.0e-12)
35 #else
36  #define REL_ERROR2 btScalar(1.0e-6)
37 #endif
38 
39 //temp globals, to improve GJK/EPA/penetration calculations
41 int gNumGjkChecks = 0;
43 
45 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
46 m_penetrationDepthSolver(penetrationDepthSolver),
47 m_simplexSolver(simplexSolver),
48 m_minkowskiA(objectA),
49 m_minkowskiB(objectB),
50 m_shapeTypeA(objectA->getShapeType()),
51 m_shapeTypeB(objectB->getShapeType()),
52 m_marginA(objectA->getMargin()),
53 m_marginB(objectB->getMargin()),
54 m_ignoreMargin(false),
55 m_lastUsedMethod(-1),
56 m_catchDegeneracies(1),
57 m_fixContactNormalDirection(1)
58 {
59 }
60 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
62 m_penetrationDepthSolver(penetrationDepthSolver),
63 m_simplexSolver(simplexSolver),
64 m_minkowskiA(objectA),
65 m_minkowskiB(objectB),
66 m_shapeTypeA(shapeTypeA),
67 m_shapeTypeB(shapeTypeB),
68 m_marginA(marginA),
69 m_marginB(marginB),
70 m_ignoreMargin(false),
74 {
75 }
76 
77 void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
78 {
79  (void)swapResults;
80 
81  getClosestPointsNonVirtual(input,output,debugDraw);
82 }
83 
84 #ifdef __SPU__
86 #else
88 #endif
89 {
91 
92  btScalar distance=btScalar(0.);
93  btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
94 
95  btVector3 pointOnA,pointOnB;
96  btTransform localTransA = input.m_transformA;
97  btTransform localTransB = input.m_transformB;
98  btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
99  localTransA.getOrigin() -= positionOffset;
100  localTransB.getOrigin() -= positionOffset;
101 
102  bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
103 
104  btScalar marginA = m_marginA;
105  btScalar marginB = m_marginB;
106 
107  gNumGjkChecks++;
108 
109  //for CCD we don't use margins
110  if (m_ignoreMargin)
111  {
112  marginA = btScalar(0.);
113  marginB = btScalar(0.);
114  }
115 
116  m_curIter = 0;
117  int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
119 
120  bool isValid = false;
121  bool checkSimplex = false;
122  bool checkPenetration = true;
124 
125  m_lastUsedMethod = -1;
126 
127  {
128  btScalar squaredDistance = BT_LARGE_FLOAT;
129  btScalar delta = btScalar(0.);
130 
131  btScalar margin = marginA + marginB;
132 
133 
134 
135  m_simplexSolver->reset();
136 
137  for ( ; ; )
138  //while (true)
139  {
140 
141  btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
142  btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
143 
144 
147 
148  btVector3 pWorld = localTransA(pInA);
149  btVector3 qWorld = localTransB(qInB);
150 
151 
152  if (check2d)
153  {
154  pWorld[2] = 0.f;
155  qWorld[2] = 0.f;
156  }
157 
158  btVector3 w = pWorld - qWorld;
159  delta = m_cachedSeparatingAxis.dot(w);
160 
161  // potential exit, they don't overlap
162  if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
163  {
164  m_degenerateSimplex = 10;
165  checkSimplex=true;
166  //checkPenetration = false;
167  break;
168  }
169 
170  //exit 0: the new point is already in the simplex, or we didn't come any closer
171  if (m_simplexSolver->inSimplex(w))
172  {
174  checkSimplex = true;
175  break;
176  }
177  // are we getting any closer ?
178  btScalar f0 = squaredDistance - delta;
179  btScalar f1 = squaredDistance * REL_ERROR2;
180 
181  if (f0 <= f1)
182  {
183  if (f0 <= btScalar(0.))
184  {
186  } else
187  {
188  m_degenerateSimplex = 11;
189  }
190  checkSimplex = true;
191  break;
192  }
193 
194  //add current vertex to simplex
195  m_simplexSolver->addVertex(w, pWorld, qWorld);
196  btVector3 newCachedSeparatingAxis;
197 
198  //calculate the closest point to the origin (update vector v)
199  if (!m_simplexSolver->closest(newCachedSeparatingAxis))
200  {
202  checkSimplex = true;
203  break;
204  }
205 
206  if(newCachedSeparatingAxis.length2()<REL_ERROR2)
207  {
208  m_cachedSeparatingAxis = newCachedSeparatingAxis;
210  checkSimplex = true;
211  break;
212  }
213 
214  btScalar previousSquaredDistance = squaredDistance;
215  squaredDistance = newCachedSeparatingAxis.length2();
216 #if 0
217  if (squaredDistance>previousSquaredDistance)
219  {
221  squaredDistance = previousSquaredDistance;
222  checkSimplex = false;
223  break;
224  }
225 #endif //
226 
227 
228  //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
229 
230  //are we getting any closer ?
231  if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
232  {
233 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
234  checkSimplex = true;
235  m_degenerateSimplex = 12;
236 
237  break;
238  }
239 
240  m_cachedSeparatingAxis = newCachedSeparatingAxis;
241 
242  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
243  if (m_curIter++ > gGjkMaxIter)
244  {
245  #if defined(DEBUG) || defined (_DEBUG)
246 
247  printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
248  printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
252  squaredDistance,
255 
256  #endif
257  break;
258 
259  }
260 
261 
262  bool check = (!m_simplexSolver->fullSimplex());
263  //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
264 
265  if (!check)
266  {
267  //do we need this backup_closest here ?
268 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
269  m_degenerateSimplex = 13;
270  break;
271  }
272  }
273 
274  if (checkSimplex)
275  {
276  m_simplexSolver->compute_points(pointOnA, pointOnB);
277  normalInB = m_cachedSeparatingAxis;
278 
280 
281  //valid normal
282  if (lenSqr < 0.0001)
283  {
285  }
286  if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
287  {
288  btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
289  normalInB *= rlen; //normalize
290 
291  btScalar s = btSqrt(squaredDistance);
292 
293  btAssert(s > btScalar(0.0));
294  pointOnA -= m_cachedSeparatingAxis * (marginA / s);
295  pointOnB += m_cachedSeparatingAxis * (marginB / s);
296  distance = ((btScalar(1.)/rlen) - margin);
297  isValid = true;
298 
299  m_lastUsedMethod = 1;
300  } else
301  {
302  m_lastUsedMethod = 2;
303  }
304  }
305 
306  bool catchDegeneratePenetrationCase =
308 
309  //if (checkPenetration && !isValid)
310  if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
311  {
312  //penetration case
313 
314  //if there is no way to handle penetrations, bail out
316  {
317  // Penetration depth case.
318  btVector3 tmpPointOnA,tmpPointOnB;
319 
322 
323  bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
324  *m_simplexSolver,
326  localTransA,localTransB,
327  m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
328  debugDraw
329  );
330 
331 
332  if (isValid2)
333  {
334  btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
335  btScalar lenSqr = tmpNormalInB.length2();
336  if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
337  {
338  tmpNormalInB = m_cachedSeparatingAxis;
339  lenSqr = m_cachedSeparatingAxis.length2();
340  }
341 
342  if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
343  {
344  tmpNormalInB /= btSqrt(lenSqr);
345  btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
346  m_lastUsedMethod = 3;
347  //only replace valid penetrations when the result is deeper (check)
348  if (!isValid || (distance2 < distance))
349  {
350  distance = distance2;
351  pointOnA = tmpPointOnA;
352  pointOnB = tmpPointOnB;
353  normalInB = tmpNormalInB;
358  {
359  btScalar d1=0;
360  {
361  btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
362  btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
363 
364 
367 
368  btVector3 pWorld = localTransA(pInA);
369  btVector3 qWorld = localTransB(qInB);
370  btVector3 w = pWorld - qWorld;
371  d1 = (-normalInB).dot(w);
372  }
373  btScalar d0 = 0.f;
374  {
375  btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
376  btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
377 
378 
381 
382  btVector3 pWorld = localTransA(pInA);
383  btVector3 qWorld = localTransB(qInB);
384  btVector3 w = pWorld - qWorld;
385  d0 = normalInB.dot(w);
386  }
387  if (d1>d0)
388  {
389  m_lastUsedMethod = 10;
390  normalInB*=-1;
391  }
392 
393  }
394  isValid = true;
395 
396  } else
397  {
398  m_lastUsedMethod = 8;
399  }
400  } else
401  {
402  m_lastUsedMethod = 9;
403  }
404  } else
405 
406  {
412 
413 
415  {
416  btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
417  //only replace valid distances when the distance is less
418  if (!isValid || (distance2 < distance))
419  {
420  distance = distance2;
421  pointOnA = tmpPointOnA;
422  pointOnB = tmpPointOnB;
423  pointOnA -= m_cachedSeparatingAxis * marginA ;
424  pointOnB += m_cachedSeparatingAxis * marginB ;
425  normalInB = m_cachedSeparatingAxis;
426  normalInB.normalize();
427 
428  isValid = true;
429  m_lastUsedMethod = 6;
430  } else
431  {
432  m_lastUsedMethod = 5;
433  }
434  }
435  }
436 
437  }
438 
439  }
440  }
441 
442 
443 
444  if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
445  {
446 
447  m_cachedSeparatingAxis = normalInB;
448  m_cachedSeparatingDistance = distance;
449 
450  output.addContactPoint(
451  normalInB,
452  pointOnB+positionOffset,
453  distance);
454 
455  }
456 
457 
458 }
459 
460 
461 
462 
463 
btConvexPenetrationDepthSolver * m_penetrationDepthSolver
btVector3 m_cachedSeparatingAxis
btSimplexSolverInterface * m_simplexSolver
btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
#define SIMD_EPSILON
Definition: btScalar.h:495
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:856
#define BT_LARGE_FLOAT
Definition: btScalar.h:281
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:650
btScalar m_cachedSeparatingDistance
btScalar gGjkEpaPenetrationTolerance
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btScalar btSqrt(btScalar y)
Definition: btScalar.h:419
#define btAssert(x)
Definition: btScalar.h:114
bool isConvex2d() const
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:573
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:307
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
int gNumDeepPenetrationChecks
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:575
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)=0
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
#define btSimplexSolverInterface
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
#define output
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
void setZero()
Definition: btVector3.h:681
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
const btConvexShape * m_minkowskiB
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
#define REL_ERROR2
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)=0
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:848
int getShapeType() const
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
const btConvexShape * m_minkowskiA
int gNumGjkChecks
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:571
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
void getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)