Blender V2.61 - r43446

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