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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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 #define REL_ERROR2 btScalar(1.0e-6)
34
35 //temp globals, to improve GJK/EPA/penetration calculations
36 int gNumDeepPenetrationChecks = 0;
37 int gNumGjkChecks = 0;
38
39
40 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
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)
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 m_fixContactNormalDirection(1)
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__
81 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
82 #else
83 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
84 #endif
85 {
86 m_cachedSeparatingDistance = 0.f;
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?
114 m_cachedSeparatingAxis.setValue(0,1,0);
115
116 bool isValid = false;
117 bool checkSimplex = false;
118 bool checkPenetration = true;
119 m_degenerateSimplex = 0;
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
141 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
142 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
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 {
169 m_degenerateSimplex = 1;
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 {
181 m_degenerateSimplex = 2;
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 {
197 m_degenerateSimplex = 3;
198 checkSimplex = true;
199 break;
200 }
201
202 if(newCachedSeparatingAxis.length2()<REL_ERROR2)
203 {
204 m_cachedSeparatingAxis = newCachedSeparatingAxis;
205 m_degenerateSimplex = 6;
206 checkSimplex = true;
207 break;
208 }
209
210 btScalar previousSquaredDistance = squaredDistance;
211 squaredDistance = newCachedSeparatingAxis.length2();
212 #if 0
213 if (squaredDistance>previousSquaredDistance)
215 {
216 m_degenerateSimplex = 7;
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",
245 m_cachedSeparatingAxis.getX(),
246 m_cachedSeparatingAxis.getY(),
247 m_cachedSeparatingAxis.getZ(),
248 squaredDistance,
249 m_minkowskiA->getShapeType(),
250 m_minkowskiB->getShapeType());
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
275 btScalar lenSqr =m_cachedSeparatingAxis.length2();
276
277 //valid normal
278 if (lenSqr < 0.0001)
279 {
280 m_degenerateSimplex = 5;
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
311 if (m_penetrationDepthSolver)
312 {
313 // Penetration depth case.
314 btVector3 tmpPointOnA,tmpPointOnB;
315
316 gNumDeepPenetrationChecks++;
317 m_cachedSeparatingAxis.setZero();
318
319 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
320 *m_simplexSolver,
321 m_minkowskiA,m_minkowskiB,
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 m_lastUsedMethod = 3;
343 //only replace valid penetrations when the result is deeper (check)
344 if (!isValid || (distance2 < distance))
345 {
346 distance = distance2;
347 pointOnA = tmpPointOnA;
348 pointOnB = tmpPointOnB;
349 normalInB = tmpNormalInB;
354 {
355 btScalar d1=0;
356 {
357 btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
358 btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
359
360
361 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
362 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
363
364 btVector3 pWorld = localTransA(pInA);
365 btVector3 qWorld = localTransB(qInB);
366 btVector3 w = pWorld - qWorld;
367 d1 = (-normalInB).dot(w);
368 }
369 btScalar d0 = 0.f;
370 {
371 btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
372 btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
373
374
375 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
376 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
377
378 btVector3 pWorld = localTransA(pInA);
379 btVector3 qWorld = localTransB(qInB);
380 btVector3 w = pWorld - qWorld;
381 d0 = normalInB.dot(w);
382 }
383 if (d1>d0)
384 {
385 m_lastUsedMethod = 10;
386 normalInB*=-1;
387 }
388
389 }
390 isValid = true;
391
392 } else
393 {
394 m_lastUsedMethod = 8;
395 }
396 } else
397 {
398 m_lastUsedMethod = 9;
399 }
400 } else
401
402 {
408
409
410 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
411 {
412 btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
413 //only replace valid distances when the distance is less
414 if (!isValid || (distance2 < distance))
415 {
416 distance = distance2;
417 pointOnA = tmpPointOnA;
418 pointOnB = tmpPointOnB;
419 pointOnA -= m_cachedSeparatingAxis * marginA ;
420 pointOnB += m_cachedSeparatingAxis * marginB ;
421 normalInB = m_cachedSeparatingAxis;
422 normalInB.normalize();
423
424 isValid = true;
425 m_lastUsedMethod = 6;
426 } else
427 {
428 m_lastUsedMethod = 5;
429 }
430 }
431 }
432
433 }
434
435 }
436 }
437
438
439
440 if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
441 {
442
443 m_cachedSeparatingAxis = normalInB;
444 m_cachedSeparatingDistance = distance;
445
446 output.addContactPoint(
447 normalInB,
448 pointOnB+positionOffset,
449 distance);
450
451 }
452
453
454 }
455
456
457
458
459
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:40
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:853
btCollisionShape::getShapeType
int getShapeType() const
Definition: btCollisionShape.h:112
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:280
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btGjkPairDetector::m_cachedSeparatingDistance
btScalar m_cachedSeparatingDistance
Definition: btGjkPairDetector.h:45
btDiscreteCollisionDetectorInterface::Result
Definition: btDiscreteCollisionDetectorInterface.h:31
btConvexPenetrationDepthSolver
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
Definition: btConvexPenetrationDepthSolver.h:26
btGjkPairDetector::m_shapeTypeA
int m_shapeTypeA
Definition: btGjkPairDetector.h:39
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btGjkPairDetector::m_degenerateSimplex
int m_degenerateSimplex
Definition: btGjkPairDetector.h:53
btConvexShape::localGetSupportVertexWithoutMarginNonVirtual
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
Definition: btConvexShape.cpp:129
btGjkPairDetector::m_ignoreMargin
bool m_ignoreMargin
Definition: btGjkPairDetector.h:44
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:49
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btConvexShape.h
btGjkPairDetector.h
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition: btDiscreteCollisionDetectorInterface.h:50
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
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:36
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
output
#define output
btGjkPairDetector::m_fixContactNormalDirection
int m_fixContactNormalDirection
Definition: btGjkPairDetector.h:55
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btVector3::setZero
void setZero()
Definition: btVector3.h:671
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
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
REL_ERROR2
#define REL_ERROR2
Definition: btGjkPairDetector.cpp:33
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
btCollisionShape::isConvex2d
bool isConvex2d() const
Definition: btCollisionShape.h:68
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:845
btGjkPairDetector::m_marginB
btScalar m_marginB
Definition: btGjkPairDetector.h:42
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:37
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btGjkPairDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition: btGjkPairDetector.cpp:73
btGjkPairDetector::getClosestPointsNonVirtual
void getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
Definition: btGjkPairDetector.cpp:83