btGjkPairDetector.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
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 //#define TEST_NON_VIRTUAL 1
00025 #include <stdio.h> //for debug printf
00026 #ifdef __SPU__
00027 #include <spu_printf.h>
00028 #define printf spu_printf
00029 //#define DEBUG_SPU_COLLISION_DETECTION 1
00030 #endif //__SPU__
00031 #endif
00032 
00033 //must be above the machine epsilon
00034 #define REL_ERROR2 btScalar(1.0e-6)
00035 
00036 //temp globals, to improve GJK/EPA/penetration calculations
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         //for CCD we don't use margins
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;//this is to catch invalid input, perhaps check for #NaN?
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                 //while (true)
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 //                      btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
00150 //                      btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
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                         // potential exit, they don't overlap
00186                         if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
00187                         {
00188                                 m_degenerateSimplex = 10;
00189                                 checkSimplex=true;
00190                                 //checkPenetration = false;
00191                                 break;
00192                         }
00193 
00194                         //exit 0: the new point is already in the simplex, or we didn't come any closer
00195                         if (m_simplexSolver->inSimplex(w))
00196                         {
00197                                 m_degenerateSimplex = 1;
00198                                 checkSimplex = true;
00199                                 break;
00200                         }
00201                         // are we getting any closer ?
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                         //add current vertex to simplex
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                         //calculate the closest point to the origin (update vector v)
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                         //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
00259 
00260                         //are we getting any closer ?
00261                         if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
00262                         { 
00263 //                              m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
00264                                 checkSimplex = true;
00265                                 m_degenerateSimplex = 12;
00266                                 
00267                                 break;
00268                         }
00269 
00270                         m_cachedSeparatingAxis = newCachedSeparatingAxis;
00271 
00272                           //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
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                         //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
00294 
00295                         if (!check)
00296                         {
00297                                 //do we need this backup_closest here ?
00298 //                              m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
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                         //valid normal
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; //normalize
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                 //if (checkPenetration && !isValid)
00338                 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
00339                 {
00340                         //penetration case
00341 
00342                         //if there is no way to handle penetrations, bail out
00343                         if (m_penetrationDepthSolver)
00344                         {
00345                                 // Penetration depth case.
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                                                 //only replace valid penetrations when the result is deeper (check)
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                                                 //only replace valid distances when the distance is less
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 //              if (check2d)
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