00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "btGjkPairDetector.h"
00017 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00018 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
00019 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
00020
00021
00022
00023 #if defined(DEBUG) || defined (_DEBUG)
00024
00025 #include <stdio.h>
00026 #ifdef __SPU__
00027 #include <spu_printf.h>
00028 #define printf spu_printf
00029
00030 #endif //__SPU__
00031 #endif
00032
00033
00034 #define REL_ERROR2 btScalar(1.0e-6)
00035
00036
00037 int gNumDeepPenetrationChecks = 0;
00038 int gNumGjkChecks = 0;
00039
00040
00041 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
00042 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
00043 m_penetrationDepthSolver(penetrationDepthSolver),
00044 m_simplexSolver(simplexSolver),
00045 m_minkowskiA(objectA),
00046 m_minkowskiB(objectB),
00047 m_shapeTypeA(objectA->getShapeType()),
00048 m_shapeTypeB(objectB->getShapeType()),
00049 m_marginA(objectA->getMargin()),
00050 m_marginB(objectB->getMargin()),
00051 m_ignoreMargin(false),
00052 m_lastUsedMethod(-1),
00053 m_catchDegeneracies(1)
00054 {
00055 }
00056 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
00057 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
00058 m_penetrationDepthSolver(penetrationDepthSolver),
00059 m_simplexSolver(simplexSolver),
00060 m_minkowskiA(objectA),
00061 m_minkowskiB(objectB),
00062 m_shapeTypeA(shapeTypeA),
00063 m_shapeTypeB(shapeTypeB),
00064 m_marginA(marginA),
00065 m_marginB(marginB),
00066 m_ignoreMargin(false),
00067 m_lastUsedMethod(-1),
00068 m_catchDegeneracies(1)
00069 {
00070 }
00071
00072 void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
00073 {
00074 (void)swapResults;
00075
00076 getClosestPointsNonVirtual(input,output,debugDraw);
00077 }
00078
00079 #ifdef __SPU__
00080 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
00081 #else
00082 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
00083 #endif
00084 {
00085 m_cachedSeparatingDistance = 0.f;
00086
00087 btScalar distance=btScalar(0.);
00088 btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
00089 btVector3 pointOnA,pointOnB;
00090 btTransform localTransA = input.m_transformA;
00091 btTransform localTransB = input.m_transformB;
00092 btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
00093 localTransA.getOrigin() -= positionOffset;
00094 localTransB.getOrigin() -= positionOffset;
00095
00096 bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
00097
00098 btScalar marginA = m_marginA;
00099 btScalar marginB = m_marginB;
00100
00101 gNumGjkChecks++;
00102
00103 #ifdef DEBUG_SPU_COLLISION_DETECTION
00104 spu_printf("inside gjk\n");
00105 #endif
00106
00107 if (m_ignoreMargin)
00108 {
00109 marginA = btScalar(0.);
00110 marginB = btScalar(0.);
00111 #ifdef DEBUG_SPU_COLLISION_DETECTION
00112 spu_printf("ignoring margin\n");
00113 #endif
00114 }
00115
00116 m_curIter = 0;
00117 int gGjkMaxIter = 1000;
00118 m_cachedSeparatingAxis.setValue(0,1,0);
00119
00120 bool isValid = false;
00121 bool checkSimplex = false;
00122 bool checkPenetration = true;
00123 m_degenerateSimplex = 0;
00124
00125 m_lastUsedMethod = -1;
00126
00127 {
00128 btScalar squaredDistance = BT_LARGE_FLOAT;
00129 btScalar delta = btScalar(0.);
00130
00131 btScalar margin = marginA + marginB;
00132
00133
00134
00135 m_simplexSolver->reset();
00136
00137 for ( ; ; )
00138
00139 {
00140
00141 btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
00142 btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
00143
00144 #if 1
00145
00146 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
00147 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
00148
00149
00150
00151
00152 #else
00153 #ifdef __SPU__
00154 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
00155 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
00156 #else
00157 btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
00158 btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
00159 #ifdef TEST_NON_VIRTUAL
00160 btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
00161 btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
00162 btAssert((pInAv-pInA).length() < 0.0001);
00163 btAssert((qInBv-qInB).length() < 0.0001);
00164 #endif //
00165 #endif //__SPU__
00166 #endif
00167
00168
00169 btVector3 pWorld = localTransA(pInA);
00170 btVector3 qWorld = localTransB(qInB);
00171
00172 #ifdef DEBUG_SPU_COLLISION_DETECTION
00173 spu_printf("got local supporting vertices\n");
00174 #endif
00175
00176 if (check2d)
00177 {
00178 pWorld[2] = 0.f;
00179 qWorld[2] = 0.f;
00180 }
00181
00182 btVector3 w = pWorld - qWorld;
00183 delta = m_cachedSeparatingAxis.dot(w);
00184
00185
00186 if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
00187 {
00188 m_degenerateSimplex = 10;
00189 checkSimplex=true;
00190
00191 break;
00192 }
00193
00194
00195 if (m_simplexSolver->inSimplex(w))
00196 {
00197 m_degenerateSimplex = 1;
00198 checkSimplex = true;
00199 break;
00200 }
00201
00202 btScalar f0 = squaredDistance - delta;
00203 btScalar f1 = squaredDistance * REL_ERROR2;
00204
00205 if (f0 <= f1)
00206 {
00207 if (f0 <= btScalar(0.))
00208 {
00209 m_degenerateSimplex = 2;
00210 } else
00211 {
00212 m_degenerateSimplex = 11;
00213 }
00214 checkSimplex = true;
00215 break;
00216 }
00217
00218 #ifdef DEBUG_SPU_COLLISION_DETECTION
00219 spu_printf("addVertex 1\n");
00220 #endif
00221
00222 m_simplexSolver->addVertex(w, pWorld, qWorld);
00223 #ifdef DEBUG_SPU_COLLISION_DETECTION
00224 spu_printf("addVertex 2\n");
00225 #endif
00226 btVector3 newCachedSeparatingAxis;
00227
00228
00229 if (!m_simplexSolver->closest(newCachedSeparatingAxis))
00230 {
00231 m_degenerateSimplex = 3;
00232 checkSimplex = true;
00233 break;
00234 }
00235
00236 if(newCachedSeparatingAxis.length2()<REL_ERROR2)
00237 {
00238 m_cachedSeparatingAxis = newCachedSeparatingAxis;
00239 m_degenerateSimplex = 6;
00240 checkSimplex = true;
00241 break;
00242 }
00243
00244 btScalar previousSquaredDistance = squaredDistance;
00245 squaredDistance = newCachedSeparatingAxis.length2();
00246 #if 0
00247
00248 if (squaredDistance>previousSquaredDistance)
00249 {
00250 m_degenerateSimplex = 7;
00251 squaredDistance = previousSquaredDistance;
00252 checkSimplex = false;
00253 break;
00254 }
00255 #endif //
00256
00257
00258
00259
00260
00261 if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
00262 {
00263
00264 checkSimplex = true;
00265 m_degenerateSimplex = 12;
00266
00267 break;
00268 }
00269
00270 m_cachedSeparatingAxis = newCachedSeparatingAxis;
00271
00272
00273 if (m_curIter++ > gGjkMaxIter)
00274 {
00275 #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
00276
00277 printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
00278 printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
00279 m_cachedSeparatingAxis.getX(),
00280 m_cachedSeparatingAxis.getY(),
00281 m_cachedSeparatingAxis.getZ(),
00282 squaredDistance,
00283 m_minkowskiA->getShapeType(),
00284 m_minkowskiB->getShapeType());
00285
00286 #endif
00287 break;
00288
00289 }
00290
00291
00292 bool check = (!m_simplexSolver->fullSimplex());
00293
00294
00295 if (!check)
00296 {
00297
00298
00299 m_degenerateSimplex = 13;
00300 break;
00301 }
00302 }
00303
00304 if (checkSimplex)
00305 {
00306 m_simplexSolver->compute_points(pointOnA, pointOnB);
00307 normalInB = m_cachedSeparatingAxis;
00308 btScalar lenSqr =m_cachedSeparatingAxis.length2();
00309
00310
00311 if (lenSqr < 0.0001)
00312 {
00313 m_degenerateSimplex = 5;
00314 }
00315 if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
00316 {
00317 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
00318 normalInB *= rlen;
00319 btScalar s = btSqrt(squaredDistance);
00320
00321 btAssert(s > btScalar(0.0));
00322 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
00323 pointOnB += m_cachedSeparatingAxis * (marginB / s);
00324 distance = ((btScalar(1.)/rlen) - margin);
00325 isValid = true;
00326
00327 m_lastUsedMethod = 1;
00328 } else
00329 {
00330 m_lastUsedMethod = 2;
00331 }
00332 }
00333
00334 bool catchDegeneratePenetrationCase =
00335 (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
00336
00337
00338 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
00339 {
00340
00341
00342
00343 if (m_penetrationDepthSolver)
00344 {
00345
00346 btVector3 tmpPointOnA,tmpPointOnB;
00347
00348 gNumDeepPenetrationChecks++;
00349 m_cachedSeparatingAxis.setZero();
00350
00351 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
00352 *m_simplexSolver,
00353 m_minkowskiA,m_minkowskiB,
00354 localTransA,localTransB,
00355 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
00356 debugDraw,input.m_stackAlloc
00357 );
00358
00359
00360 if (isValid2)
00361 {
00362 btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
00363 btScalar lenSqr = tmpNormalInB.length2();
00364 if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
00365 {
00366 tmpNormalInB = m_cachedSeparatingAxis;
00367 lenSqr = m_cachedSeparatingAxis.length2();
00368 }
00369
00370 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
00371 {
00372 tmpNormalInB /= btSqrt(lenSqr);
00373 btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
00374
00375 if (!isValid || (distance2 < distance))
00376 {
00377 distance = distance2;
00378 pointOnA = tmpPointOnA;
00379 pointOnB = tmpPointOnB;
00380 normalInB = tmpNormalInB;
00381 isValid = true;
00382 m_lastUsedMethod = 3;
00383 } else
00384 {
00385 m_lastUsedMethod = 8;
00386 }
00387 } else
00388 {
00389 m_lastUsedMethod = 9;
00390 }
00391 } else
00392
00393 {
00399
00400
00401 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
00402 {
00403 btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
00404
00405 if (!isValid || (distance2 < distance))
00406 {
00407 distance = distance2;
00408 pointOnA = tmpPointOnA;
00409 pointOnB = tmpPointOnB;
00410 pointOnA -= m_cachedSeparatingAxis * marginA ;
00411 pointOnB += m_cachedSeparatingAxis * marginB ;
00412 normalInB = m_cachedSeparatingAxis;
00413 normalInB.normalize();
00414 isValid = true;
00415 m_lastUsedMethod = 6;
00416 } else
00417 {
00418 m_lastUsedMethod = 5;
00419 }
00420 }
00421 }
00422
00423 }
00424
00425 }
00426 }
00427
00428
00429
00430 if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
00431 {
00432 #if 0
00433
00434
00435 {
00436 printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
00437 printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
00438 }
00439 #endif
00440
00441 m_cachedSeparatingAxis = normalInB;
00442 m_cachedSeparatingDistance = distance;
00443
00444 output.addContactPoint(
00445 normalInB,
00446 pointOnB+positionOffset,
00447 distance);
00448
00449 }
00450
00451
00452 }
00453
00454
00455
00456
00457