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 #define REL_ERROR2 btScalar(1.0e-6)
34 
35 //temp globals, to improve GJK/EPA/penetration calculations
37 int gNumGjkChecks = 0;
38 
39 
41 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
42 m_penetrationDepthSolver(penetrationDepthSolver),
43 m_simplexSolver(simplexSolver),
44 m_minkowskiA(objectA),
45 m_minkowskiB(objectB),
46 m_shapeTypeA(objectA->getShapeType()),
47 m_shapeTypeB(objectB->getShapeType()),
48 m_marginA(objectA->getMargin()),
49 m_marginB(objectB->getMargin()),
50 m_ignoreMargin(false),
51 m_lastUsedMethod(-1),
52 m_catchDegeneracies(1),
53 m_fixContactNormalDirection(1)
54 {
55 }
56 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
58 m_penetrationDepthSolver(penetrationDepthSolver),
59 m_simplexSolver(simplexSolver),
60 m_minkowskiA(objectA),
61 m_minkowskiB(objectB),
62 m_shapeTypeA(shapeTypeA),
63 m_shapeTypeB(shapeTypeB),
64 m_marginA(marginA),
65 m_marginB(marginB),
66 m_ignoreMargin(false),
70 {
71 }
72 
73 void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
74 {
75  (void)swapResults;
76 
77  getClosestPointsNonVirtual(input,output,debugDraw);
78 }
79 
80 #ifdef __SPU__
82 #else
84 #endif
85 {
87 
88  btScalar distance=btScalar(0.);
89  btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
90 
91  btVector3 pointOnA,pointOnB;
92  btTransform localTransA = input.m_transformA;
93  btTransform localTransB = input.m_transformB;
94  btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
95  localTransA.getOrigin() -= positionOffset;
96  localTransB.getOrigin() -= positionOffset;
97 
98  bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
99 
100  btScalar marginA = m_marginA;
101  btScalar marginB = m_marginB;
102 
103  gNumGjkChecks++;
104 
105  //for CCD we don't use margins
106  if (m_ignoreMargin)
107  {
108  marginA = btScalar(0.);
109  marginB = btScalar(0.);
110  }
111 
112  m_curIter = 0;
113  int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
115 
116  bool isValid = false;
117  bool checkSimplex = false;
118  bool checkPenetration = true;
120 
121  m_lastUsedMethod = -1;
122 
123  {
124  btScalar squaredDistance = BT_LARGE_FLOAT;
125  btScalar delta = btScalar(0.);
126 
127  btScalar margin = marginA + marginB;
128 
129 
130 
131  m_simplexSolver->reset();
132 
133  for ( ; ; )
134  //while (true)
135  {
136 
137  btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
138  btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
139 
140 
143 
144  btVector3 pWorld = localTransA(pInA);
145  btVector3 qWorld = localTransB(qInB);
146 
147 
148  if (check2d)
149  {
150  pWorld[2] = 0.f;
151  qWorld[2] = 0.f;
152  }
153 
154  btVector3 w = pWorld - qWorld;
155  delta = m_cachedSeparatingAxis.dot(w);
156 
157  // potential exit, they don't overlap
158  if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
159  {
160  m_degenerateSimplex = 10;
161  checkSimplex=true;
162  //checkPenetration = false;
163  break;
164  }
165 
166  //exit 0: the new point is already in the simplex, or we didn't come any closer
167  if (m_simplexSolver->inSimplex(w))
168  {
170  checkSimplex = true;
171  break;
172  }
173  // are we getting any closer ?
174  btScalar f0 = squaredDistance - delta;
175  btScalar f1 = squaredDistance * REL_ERROR2;
176 
177  if (f0 <= f1)
178  {
179  if (f0 <= btScalar(0.))
180  {
182  } else
183  {
184  m_degenerateSimplex = 11;
185  }
186  checkSimplex = true;
187  break;
188  }
189 
190  //add current vertex to simplex
191  m_simplexSolver->addVertex(w, pWorld, qWorld);
192  btVector3 newCachedSeparatingAxis;
193 
194  //calculate the closest point to the origin (update vector v)
195  if (!m_simplexSolver->closest(newCachedSeparatingAxis))
196  {
198  checkSimplex = true;
199  break;
200  }
201 
202  if(newCachedSeparatingAxis.length2()<REL_ERROR2)
203  {
204  m_cachedSeparatingAxis = newCachedSeparatingAxis;
206  checkSimplex = true;
207  break;
208  }
209 
210  btScalar previousSquaredDistance = squaredDistance;
211  squaredDistance = newCachedSeparatingAxis.length2();
212 #if 0
213  if (squaredDistance>previousSquaredDistance)
215  {
217  squaredDistance = previousSquaredDistance;
218  checkSimplex = false;
219  break;
220  }
221 #endif //
222 
223 
224  //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
225 
226  //are we getting any closer ?
227  if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
228  {
229 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
230  checkSimplex = true;
231  m_degenerateSimplex = 12;
232 
233  break;
234  }
235 
236  m_cachedSeparatingAxis = newCachedSeparatingAxis;
237 
238  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
239  if (m_curIter++ > gGjkMaxIter)
240  {
241  #if defined(DEBUG) || defined (_DEBUG)
242 
243  printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
244  printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
248  squaredDistance,
251 
252  #endif
253  break;
254 
255  }
256 
257 
258  bool check = (!m_simplexSolver->fullSimplex());
259  //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
260 
261  if (!check)
262  {
263  //do we need this backup_closest here ?
264 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
265  m_degenerateSimplex = 13;
266  break;
267  }
268  }
269 
270  if (checkSimplex)
271  {
272  m_simplexSolver->compute_points(pointOnA, pointOnB);
273  normalInB = m_cachedSeparatingAxis;
274 
276 
277  //valid normal
278  if (lenSqr < 0.0001)
279  {
281  }
282  if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
283  {
284  btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
285  normalInB *= rlen; //normalize
286 
287  btScalar s = btSqrt(squaredDistance);
288 
289  btAssert(s > btScalar(0.0));
290  pointOnA -= m_cachedSeparatingAxis * (marginA / s);
291  pointOnB += m_cachedSeparatingAxis * (marginB / s);
292  distance = ((btScalar(1.)/rlen) - margin);
293  isValid = true;
294 
295  m_lastUsedMethod = 1;
296  } else
297  {
298  m_lastUsedMethod = 2;
299  }
300  }
301 
302  bool catchDegeneratePenetrationCase =
303  (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
304 
305  //if (checkPenetration && !isValid)
306  if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
307  {
308  //penetration case
309 
310  //if there is no way to handle penetrations, bail out
312  {
313  // Penetration depth case.
314  btVector3 tmpPointOnA,tmpPointOnB;
315 
318 
319  bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
320  *m_simplexSolver,
322  localTransA,localTransB,
323  m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
324  debugDraw
325  );
326 
327 
328  if (isValid2)
329  {
330  btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
331  btScalar lenSqr = tmpNormalInB.length2();
332  if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
333  {
334  tmpNormalInB = m_cachedSeparatingAxis;
335  lenSqr = m_cachedSeparatingAxis.length2();
336  }
337 
338  if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
339  {
340  tmpNormalInB /= btSqrt(lenSqr);
341  btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
342  //only replace valid penetrations when the result is deeper (check)
343  if (!isValid || (distance2 < distance))
344  {
345  distance = distance2;
346  pointOnA = tmpPointOnA;
347  pointOnB = tmpPointOnB;
348  normalInB = tmpNormalInB;
349 
350  isValid = true;
351  m_lastUsedMethod = 3;
352  } else
353  {
354  m_lastUsedMethod = 8;
355  }
356  } else
357  {
358  m_lastUsedMethod = 9;
359  }
360  } else
361 
362  {
368 
369 
371  {
372  btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
373  //only replace valid distances when the distance is less
374  if (!isValid || (distance2 < distance))
375  {
376  distance = distance2;
377  pointOnA = tmpPointOnA;
378  pointOnB = tmpPointOnB;
379  pointOnA -= m_cachedSeparatingAxis * marginA ;
380  pointOnB += m_cachedSeparatingAxis * marginB ;
381  normalInB = m_cachedSeparatingAxis;
382  normalInB.normalize();
383 
384  isValid = true;
385  m_lastUsedMethod = 6;
386  } else
387  {
388  m_lastUsedMethod = 5;
389  }
390  }
391  }
392 
393  }
394 
395  }
396  }
397 
398 
399 
400  if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
401  {
402 
403  m_cachedSeparatingAxis = normalInB;
404  m_cachedSeparatingDistance = distance;
405 
406  output.addContactPoint(
407  normalInB,
408  pointOnB+positionOffset,
409  distance);
410 
411  }
412 
413 
414 }
415 
416 
417 
418 
419 
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:494
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:835
#define BT_LARGE_FLOAT
Definition: btScalar.h:280
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar m_cachedSeparatingDistance
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:418
#define btAssert(x)
Definition: btScalar.h:113
bool isConvex2d() const
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
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:565
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:671
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:28
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
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:561
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
void getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)