Bullet Collision Detection & Physics Library: btGjkPairDetector.cpp Source File

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 #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
40 int gNumDeepPenetrationChecks = 0;
41 int gNumGjkChecks = 0;
42 btScalar gGjkEpaPenetrationTolerance = 0.001;
43
44 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
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)
61 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
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),
71 m_lastUsedMethod(-1),
72 m_catchDegeneracies(1),
73 m_fixContactNormalDirection(1)
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__
85 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
86 #else
87 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
88 #endif
89 {
90 m_cachedSeparatingDistance = 0.f;
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?
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
145 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
146 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
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 {
173 m_degenerateSimplex = 1;
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 {
185 m_degenerateSimplex = 2;
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 {
201 m_degenerateSimplex = 3;
202 checkSimplex = true;
203 break;
204 }
205
206 if(newCachedSeparatingAxis.length2()<REL_ERROR2)
207 {
208 m_cachedSeparatingAxis = newCachedSeparatingAxis;
209 m_degenerateSimplex = 6;
210 checkSimplex = true;
211 break;
212 }
213
214 btScalar previousSquaredDistance = squaredDistance;
215 squaredDistance = newCachedSeparatingAxis.length2();
216 #if 0
217 if (squaredDistance>previousSquaredDistance)
219 {
220 m_degenerateSimplex = 7;
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",
249 m_cachedSeparatingAxis.getX(),
250 m_cachedSeparatingAxis.getY(),
251 m_cachedSeparatingAxis.getZ(),
252 squaredDistance,
253 m_minkowskiA->getShapeType(),
254 m_minkowskiB->getShapeType());
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
279 btScalar lenSqr =m_cachedSeparatingAxis.length2();
280
281 //valid normal
282 if (lenSqr < 0.0001)
283 {
284 m_degenerateSimplex = 5;
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 =
307 (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
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
315 if (m_penetrationDepthSolver)
316 {
317 // Penetration depth case.
318 btVector3 tmpPointOnA,tmpPointOnB;
319
320 gNumDeepPenetrationChecks++;
321 m_cachedSeparatingAxis.setZero();
322
323 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
324 *m_simplexSolver,
325 m_minkowskiA,m_minkowskiB,
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
365 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
366 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
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
379 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
380 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
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
414 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
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
btGjkPairDetector::m_penetrationDepthSolver
btConvexPenetrationDepthSolver * m_penetrationDepthSolver
Definition: btGjkPairDetector.h:35
btGjkPairDetector::m_cachedSeparatingAxis
btVector3 m_cachedSeparatingAxis
Definition: btGjkPairDetector.h:34
btGjkPairDetector::m_simplexSolver
btSimplexSolverInterface * m_simplexSolver
Definition: btGjkPairDetector.h:36
btGjkPairDetector::btGjkPairDetector
btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
Definition: btGjkPairDetector.cpp:44
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:495
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:856
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:281
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:650
btGjkPairDetector::m_cachedSeparatingDistance
btScalar m_cachedSeparatingDistance
Definition: btGjkPairDetector.h:45
btDiscreteCollisionDetectorInterface::Result
Definition: btDiscreteCollisionDetectorInterface.h:31
gGjkEpaPenetrationTolerance
btScalar gGjkEpaPenetrationTolerance
Definition: btGjkPairDetector.cpp:42
btConvexPenetrationDepthSolver
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
Definition: btConvexPenetrationDepthSolver.h:26
btGjkPairDetector::m_shapeTypeA
int m_shapeTypeA
Definition: btGjkPairDetector.h:39
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:419
btAssert
#define btAssert(x)
Definition: btScalar.h:114
btGjkPairDetector::m_degenerateSimplex
int m_degenerateSimplex
Definition: btGjkPairDetector.h:53
btCollisionShape::isConvex2d
bool isConvex2d() const
Definition: btCollisionShape.h:68
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:573
btGjkPairDetector::m_ignoreMargin
bool m_ignoreMargin
Definition: btGjkPairDetector.h:44
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:49
btConvexShape.h
btGjkPairDetector.h
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:307
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition: btDiscreteCollisionDetectorInterface.h:50
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
gNumDeepPenetrationChecks
int gNumDeepPenetrationChecks
Definition: btGjkPairDetector.cpp:40
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:575
btConvexPenetrationDepthSolver::calcPenDepth
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
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition: btDiscreteCollisionDetectorInterface.h:51
btGjkPairDetector::m_catchDegeneracies
int m_catchDegeneracies
Definition: btGjkPairDetector.h:54
btGjkPairDetector::m_lastUsedMethod
int m_lastUsedMethod
Definition: btGjkPairDetector.h:51
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btSimplexSolverInterface
#define btSimplexSolverInterface
Definition: btSimplexSolverInterface.h:26
btGjkPairDetector::m_curIter
int m_curIter
Definition: btGjkPairDetector.h:52
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
output
#define output
btGjkPairDetector::m_fixContactNormalDirection
int m_fixContactNormalDirection
Definition: btGjkPairDetector.h:55
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btVector3::setZero
void setZero()
Definition: btVector3.h:681
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
btGjkPairDetector::m_minkowskiB
const btConvexShape * m_minkowskiB
Definition: btGjkPairDetector.h:38
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
REL_ERROR2
#define REL_ERROR2
Definition: btGjkPairDetector.cpp:36
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btDiscreteCollisionDetectorInterface::Result::addContactPoint
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)=0
btConvexPenetrationDepthSolver.h
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:848
btCollisionShape::getShapeType
int getShapeType() const
Definition: btCollisionShape.h:112
btGjkPairDetector::m_marginB
btScalar m_marginB
Definition: btGjkPairDetector.h:42
btConvexShape::localGetSupportVertexWithoutMarginNonVirtual
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
Definition: btConvexShape.cpp:129
btGjkPairDetector::m_shapeTypeB
int m_shapeTypeB
Definition: btGjkPairDetector.h:40
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition: btDiscreteCollisionDetectorInterface.h:42
btSimplexSolverInterface.h
btGjkPairDetector::m_marginA
btScalar m_marginA
Definition: btGjkPairDetector.h:41
btGjkPairDetector::m_minkowskiA
const btConvexShape * m_minkowskiA
Definition: btGjkPairDetector.h:37
gNumGjkChecks
int gNumGjkChecks
Definition: btGjkPairDetector.cpp:41
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:571
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btGjkPairDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition: btGjkPairDetector.cpp:77
btGjkPairDetector::getClosestPointsNonVirtual
void getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
Definition: btGjkPairDetector.cpp:87