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"
17 #include "BulletCollision/CollisionShapes/btConvexShape.h"
18 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
19 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.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 //#define DEBUG_SPU_COLLISION_DETECTION 1
30 #endif //__SPU__
31 #endif
32 
33 //must be above the machine epsilon
34 #define REL_ERROR2 btScalar(1.0e-6)
35 
36 //temp globals, to improve GJK/EPA/penetration calculations
37 int gNumDeepPenetrationChecks = 0;
38 int gNumGjkChecks = 0;
39 
40 
41 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
42 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
43 m_penetrationDepthSolver(penetrationDepthSolver),
44 m_simplexSolver(simplexSolver),
45 m_minkowskiA(objectA),
46 m_minkowskiB(objectB),
47 m_shapeTypeA(objectA->getShapeType()),
48 m_shapeTypeB(objectB->getShapeType()),
49 m_marginA(objectA->getMargin()),
50 m_marginB(objectB->getMargin()),
51 m_ignoreMargin(false),
52 m_lastUsedMethod(-1),
53 m_catchDegeneracies(1)
54 {
55 }
56 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
57 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
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),
67 m_lastUsedMethod(-1),
68 m_catchDegeneracies(1)
69 {
70 }
71 
72 void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
73 {
74  (void)swapResults;
75 
76  getClosestPointsNonVirtual(input,output,debugDraw);
77 }
78 
79 #ifdef __SPU__
80 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
81 #else
82 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
83 #endif
84 {
85  m_cachedSeparatingDistance = 0.f;
86 
87  btScalar distance=btScalar(0.);
88  btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
89  btVector3 pointOnA,pointOnB;
90  btTransform localTransA = input.m_transformA;
91  btTransform localTransB = input.m_transformB;
92  btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
93  localTransA.getOrigin() -= positionOffset;
94  localTransB.getOrigin() -= positionOffset;
95 
96  bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
97 
98  btScalar marginA = m_marginA;
99  btScalar marginB = m_marginB;
100 
101  gNumGjkChecks++;
102 
103 #ifdef DEBUG_SPU_COLLISION_DETECTION
104  spu_printf("inside gjk\n");
105 #endif
106  //for CCD we don't use margins
107  if (m_ignoreMargin)
108  {
109  marginA = btScalar(0.);
110  marginB = btScalar(0.);
111 #ifdef DEBUG_SPU_COLLISION_DETECTION
112  spu_printf("ignoring margin\n");
113 #endif
114  }
115 
116  m_curIter = 0;
117  int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
118  m_cachedSeparatingAxis.setValue(0,1,0);
119 
120  bool isValid = false;
121  bool checkSimplex = false;
122  bool checkPenetration = true;
123  m_degenerateSimplex = 0;
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 #if 1
145 
146  btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
147  btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
148 
149 // btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
150 // btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
151 
152 #else
153 #ifdef __SPU__
154  btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
155  btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
156 #else
157  btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
158  btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
159 #ifdef TEST_NON_VIRTUAL
160  btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
161  btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
162  btAssert((pInAv-pInA).length() < 0.0001);
163  btAssert((qInBv-qInB).length() < 0.0001);
164 #endif //
165 #endif //__SPU__
166 #endif
167 
168 
169  btVector3 pWorld = localTransA(pInA);
170  btVector3 qWorld = localTransB(qInB);
171 
172 #ifdef DEBUG_SPU_COLLISION_DETECTION
173  spu_printf("got local supporting vertices\n");
174 #endif
175 
176  if (check2d)
177  {
178  pWorld[2] = 0.f;
179  qWorld[2] = 0.f;
180  }
181 
182  btVector3 w = pWorld - qWorld;
183  delta = m_cachedSeparatingAxis.dot(w);
184 
185  // potential exit, they don't overlap
186  if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
187  {
188  m_degenerateSimplex = 10;
189  checkSimplex=true;
190  //checkPenetration = false;
191  break;
192  }
193 
194  //exit 0: the new point is already in the simplex, or we didn't come any closer
195  if (m_simplexSolver->inSimplex(w))
196  {
197  m_degenerateSimplex = 1;
198  checkSimplex = true;
199  break;
200  }
201  // are we getting any closer ?
202  btScalar f0 = squaredDistance - delta;
203  btScalar f1 = squaredDistance * REL_ERROR2;
204 
205  if (f0 <= f1)
206  {
207  if (f0 <= btScalar(0.))
208  {
209  m_degenerateSimplex = 2;
210  } else
211  {
212  m_degenerateSimplex = 11;
213  }
214  checkSimplex = true;
215  break;
216  }
217 
218 #ifdef DEBUG_SPU_COLLISION_DETECTION
219  spu_printf("addVertex 1\n");
220 #endif
221  //add current vertex to simplex
222  m_simplexSolver->addVertex(w, pWorld, qWorld);
223 #ifdef DEBUG_SPU_COLLISION_DETECTION
224  spu_printf("addVertex 2\n");
225 #endif
226  btVector3 newCachedSeparatingAxis;
227 
228  //calculate the closest point to the origin (update vector v)
229  if (!m_simplexSolver->closest(newCachedSeparatingAxis))
230  {
231  m_degenerateSimplex = 3;
232  checkSimplex = true;
233  break;
234  }
235 
236  if(newCachedSeparatingAxis.length2()<REL_ERROR2)
237  {
238  m_cachedSeparatingAxis = newCachedSeparatingAxis;
239  m_degenerateSimplex = 6;
240  checkSimplex = true;
241  break;
242  }
243 
244  btScalar previousSquaredDistance = squaredDistance;
245  squaredDistance = newCachedSeparatingAxis.length2();
246 #if 0
247  if (squaredDistance>previousSquaredDistance)
249  {
250  m_degenerateSimplex = 7;
251  squaredDistance = previousSquaredDistance;
252  checkSimplex = false;
253  break;
254  }
255 #endif //
256 
257 
258  //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
259 
260  //are we getting any closer ?
261  if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
262  {
263 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
264  checkSimplex = true;
265  m_degenerateSimplex = 12;
266 
267  break;
268  }
269 
270  m_cachedSeparatingAxis = newCachedSeparatingAxis;
271 
272  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
273  if (m_curIter++ > gGjkMaxIter)
274  {
275  #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
276 
277  printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
278  printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
279  m_cachedSeparatingAxis.getX(),
280  m_cachedSeparatingAxis.getY(),
281  m_cachedSeparatingAxis.getZ(),
282  squaredDistance,
283  m_minkowskiA->getShapeType(),
284  m_minkowskiB->getShapeType());
285 
286  #endif
287  break;
288 
289  }
290 
291 
292  bool check = (!m_simplexSolver->fullSimplex());
293  //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
294 
295  if (!check)
296  {
297  //do we need this backup_closest here ?
298 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
299  m_degenerateSimplex = 13;
300  break;
301  }
302  }
303 
304  if (checkSimplex)
305  {
306  m_simplexSolver->compute_points(pointOnA, pointOnB);
307  normalInB = m_cachedSeparatingAxis;
308  btScalar lenSqr =m_cachedSeparatingAxis.length2();
309 
310  //valid normal
311  if (lenSqr < 0.0001)
312  {
313  m_degenerateSimplex = 5;
314  }
315  if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
316  {
317  btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
318  normalInB *= rlen; //normalize
319  btScalar s = btSqrt(squaredDistance);
320 
321  btAssert(s > btScalar(0.0));
322  pointOnA -= m_cachedSeparatingAxis * (marginA / s);
323  pointOnB += m_cachedSeparatingAxis * (marginB / s);
324  distance = ((btScalar(1.)/rlen) - margin);
325  isValid = true;
326 
327  m_lastUsedMethod = 1;
328  } else
329  {
330  m_lastUsedMethod = 2;
331  }
332  }
333 
334  bool catchDegeneratePenetrationCase =
335  (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
336 
337  //if (checkPenetration && !isValid)
338  if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
339  {
340  //penetration case
341 
342  //if there is no way to handle penetrations, bail out
343  if (m_penetrationDepthSolver)
344  {
345  // Penetration depth case.
346  btVector3 tmpPointOnA,tmpPointOnB;
347 
348  gNumDeepPenetrationChecks++;
349  m_cachedSeparatingAxis.setZero();
350 
351  bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
352  *m_simplexSolver,
353  m_minkowskiA,m_minkowskiB,
354  localTransA,localTransB,
355  m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
356  debugDraw,input.m_stackAlloc
357  );
358 
359 
360  if (isValid2)
361  {
362  btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
363  btScalar lenSqr = tmpNormalInB.length2();
364  if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
365  {
366  tmpNormalInB = m_cachedSeparatingAxis;
367  lenSqr = m_cachedSeparatingAxis.length2();
368  }
369 
370  if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
371  {
372  tmpNormalInB /= btSqrt(lenSqr);
373  btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
374  //only replace valid penetrations when the result is deeper (check)
375  if (!isValid || (distance2 < distance))
376  {
377  distance = distance2;
378  pointOnA = tmpPointOnA;
379  pointOnB = tmpPointOnB;
380  normalInB = tmpNormalInB;
381  isValid = true;
382  m_lastUsedMethod = 3;
383  } else
384  {
385  m_lastUsedMethod = 8;
386  }
387  } else
388  {
389  m_lastUsedMethod = 9;
390  }
391  } else
392 
393  {
399 
400 
401  if (m_cachedSeparatingAxis.length2() > btScalar(0.))
402  {
403  btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
404  //only replace valid distances when the distance is less
405  if (!isValid || (distance2 < distance))
406  {
407  distance = distance2;
408  pointOnA = tmpPointOnA;
409  pointOnB = tmpPointOnB;
410  pointOnA -= m_cachedSeparatingAxis * marginA ;
411  pointOnB += m_cachedSeparatingAxis * marginB ;
412  normalInB = m_cachedSeparatingAxis;
413  normalInB.normalize();
414  isValid = true;
415  m_lastUsedMethod = 6;
416  } else
417  {
418  m_lastUsedMethod = 5;
419  }
420  }
421  }
422 
423  }
424 
425  }
426  }
427 
428 
429 
430  if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
431  {
432 #if 0
433 // if (check2d)
435  {
436  printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
437  printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
438  }
439 #endif
440 
441  m_cachedSeparatingAxis = normalInB;
442  m_cachedSeparatingDistance = distance;
443 
444  output.addContactPoint(
445  normalInB,
446  pointOnB+positionOffset,
447  distance);
448 
449  }
450 
451 
452 }
453 
454 
455 
456 
457 
Generated on Thu Oct 3 2013 18:25:06 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4