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