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
btGjkPairDetector(const btConvexShape * objectA,const btConvexShape * objectB,btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * penetrationDepthSolver)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 }
btGjkPairDetector(const btConvexShape * objectA,const btConvexShape * objectB,int shapeTypeA,int shapeTypeB,btScalar marginA,btScalar marginB,btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * penetrationDepthSolver)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
getClosestPoints(const ClosestPointInput & input,Result & output,class btIDebugDraw * debugDraw,bool swapResults)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__
getClosestPointsNonVirtual(const ClosestPointInput & input,Result & output,class btIDebugDraw * debugDraw)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 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
214 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 //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 {
363 ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
364 ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
365 ///reports a valid positive distance. Use the results of the second GJK instead of failing.
366 ///thanks to Jacob.Langford for the reproduction case
367 ///http://code.google.com/p/bullet/issues/detail?id=250
368
369
370 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
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
420