Back to index

supertuxkart  0.5+dfsg1
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 #if defined(DEBUG) || defined (_DEBUG)
00022 #include <stdio.h> //for debug printf
00023 #ifdef __SPU__
00024 #include <spu_printf.h>
00025 #define printf spu_printf
00026 #endif //__SPU__
00027 #endif
00028 
00029 //must be above the machine epsilon
00030 #define REL_ERROR2 btScalar(1.0e-6)
00031 
00032 //temp globals, to improve GJK/EPA/penetration calculations
00033 int gNumDeepPenetrationChecks = 0;
00034 int gNumGjkChecks = 0;
00035 
00036 
00037 
00038 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*  penetrationDepthSolver)
00039 :m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
00040 m_penetrationDepthSolver(penetrationDepthSolver),
00041 m_simplexSolver(simplexSolver),
00042 m_minkowskiA(objectA),
00043 m_minkowskiB(objectB),
00044 m_ignoreMargin(false),
00045 m_lastUsedMethod(-1),
00046 m_catchDegeneracies(1)
00047 {
00048 }
00049 
00050 void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
00051 {
00052        btScalar distance=btScalar(0.);
00053        btVector3     normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
00054        btVector3 pointOnA,pointOnB;
00055        btTransform   localTransA = input.m_transformA;
00056        btTransform localTransB = input.m_transformB;
00057        btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
00058        localTransA.getOrigin() -= positionOffset;
00059        localTransB.getOrigin() -= positionOffset;
00060 
00061        btScalar marginA = m_minkowskiA->getMargin();
00062        btScalar marginB = m_minkowskiB->getMargin();
00063 
00064        gNumGjkChecks++;
00065 
00066        //for CCD we don't use margins
00067        if (m_ignoreMargin)
00068        {
00069               marginA = btScalar(0.);
00070               marginB = btScalar(0.);
00071        }
00072 
00073        m_curIter = 0;
00074        int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
00075        m_cachedSeparatingAxis.setValue(0,1,0);
00076 
00077        bool isValid = false;
00078        bool checkSimplex = false;
00079        bool checkPenetration = true;
00080        m_degenerateSimplex = 0;
00081 
00082        m_lastUsedMethod = -1;
00083 
00084        {
00085               btScalar squaredDistance = SIMD_INFINITY;
00086               btScalar delta = btScalar(0.);
00087               
00088               btScalar margin = marginA + marginB;
00089               
00090               
00091 
00092               m_simplexSolver->reset();
00093               
00094               for ( ; ; )
00095               //while (true)
00096               {
00097 
00098                      btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
00099                      btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
00100 
00101                      btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
00102                      btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
00103                      btPoint3  pWorld = localTransA(pInA);     
00104                      btPoint3  qWorld = localTransB(qInB);
00105                      
00106                      btVector3 w   = pWorld - qWorld;
00107                      delta = m_cachedSeparatingAxis.dot(w);
00108 
00109                      // potential exit, they don't overlap
00110                      if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
00111                      {
00112                             checkPenetration = false;
00113                             break;
00114                      }
00115 
00116                      //exit 0: the new point is already in the simplex, or we didn't come any closer
00117                      if (m_simplexSolver->inSimplex(w))
00118                      {
00119                             m_degenerateSimplex = 1;
00120                             checkSimplex = true;
00121                             break;
00122                      }
00123                      // are we getting any closer ?
00124                      btScalar f0 = squaredDistance - delta;
00125                      btScalar f1 = squaredDistance * REL_ERROR2;
00126 
00127                      if (f0 <= f1)
00128                      {
00129                             if (f0 <= btScalar(0.))
00130                             {
00131                                    m_degenerateSimplex = 2;
00132                             }
00133                             checkSimplex = true;
00134                             break;
00135                      }
00136                      //add current vertex to simplex
00137                      m_simplexSolver->addVertex(w, pWorld, qWorld);
00138 
00139                      //calculate the closest point to the origin (update vector v)
00140                      if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
00141                      {
00142                             m_degenerateSimplex = 3;
00143                             checkSimplex = true;
00144                             break;
00145                      }
00146 
00147                      btScalar previousSquaredDistance = squaredDistance;
00148                      squaredDistance = m_cachedSeparatingAxis.length2();
00149                      
00150                      //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
00151 
00152                      //are we getting any closer ?
00153                      if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
00154                      { 
00155                             m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
00156                             checkSimplex = true;
00157                             break;
00158                      }
00159 
00160                        //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
00161               if (m_curIter++ > gGjkMaxIter)   
00162               {   
00163                       #if defined(DEBUG) || defined (_DEBUG)   
00164 
00165                               printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
00166                               printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
00167                               m_cachedSeparatingAxis.getX(),   
00168                               m_cachedSeparatingAxis.getY(),   
00169                               m_cachedSeparatingAxis.getZ(),   
00170                               squaredDistance,   
00171                               m_minkowskiA->getShapeType(),   
00172                               m_minkowskiB->getShapeType());   
00173 
00174                       #endif   
00175                       break;   
00176 
00177               } 
00178 
00179 
00180                      bool check = (!m_simplexSolver->fullSimplex());
00181                      //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
00182 
00183                      if (!check)
00184                      {
00185                             //do we need this backup_closest here ?
00186                             m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
00187                             break;
00188                      }
00189               }
00190 
00191               if (checkSimplex)
00192               {
00193                      m_simplexSolver->compute_points(pointOnA, pointOnB);
00194                      normalInB = pointOnA-pointOnB;
00195                      btScalar lenSqr = m_cachedSeparatingAxis.length2();
00196                      //valid normal
00197                      if (lenSqr < 0.0001)
00198                      {
00199                             m_degenerateSimplex = 5;
00200                      } 
00201                      if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
00202                      {
00203                             btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
00204                             normalInB *= rlen; //normalize
00205                             btScalar s = btSqrt(squaredDistance);
00206                      
00207                             btAssert(s > btScalar(0.0));
00208                             pointOnA -= m_cachedSeparatingAxis * (marginA / s);
00209                             pointOnB += m_cachedSeparatingAxis * (marginB / s);
00210                             distance = ((btScalar(1.)/rlen) - margin);
00211                             isValid = true;
00212                             
00213                             m_lastUsedMethod = 1;
00214                      } else
00215                      {
00216                             m_lastUsedMethod = 2;
00217                      }
00218               }
00219 
00220               bool catchDegeneratePenetrationCase = 
00221                      (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
00222 
00223               //if (checkPenetration && !isValid)
00224               if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
00225               {
00226                      //penetration case
00227               
00228                      //if there is no way to handle penetrations, bail out
00229                      if (m_penetrationDepthSolver)
00230                      {
00231                             // Penetration depth case.
00232                             btVector3 tmpPointOnA,tmpPointOnB;
00233                             
00234                             gNumDeepPenetrationChecks++;
00235 
00236                             bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
00237                                    *m_simplexSolver, 
00238                                    m_minkowskiA,m_minkowskiB,
00239                                    localTransA,localTransB,
00240                                    m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
00241                                    debugDraw,input.m_stackAlloc
00242                                    );
00243 
00244                             if (isValid2)
00245                             {
00246                                    btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
00247                                    btScalar lenSqr = tmpNormalInB.length2();
00248                                    if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
00249                                    {
00250                                           tmpNormalInB /= btSqrt(lenSqr);
00251                                           btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
00252                                           //only replace valid penetrations when the result is deeper (check)
00253                                           if (!isValid || (distance2 < distance))
00254                                           {
00255                                                  distance = distance2;
00256                                                  pointOnA = tmpPointOnA;
00257                                                  pointOnB = tmpPointOnB;
00258                                                  normalInB = tmpNormalInB;
00259                                                  isValid = true;
00260                                                  m_lastUsedMethod = 3;
00261                                           } else
00262                                           {
00263                                                  
00264                                           }
00265                                    } else
00266                                    {
00267                                           //isValid = false;
00268                                           m_lastUsedMethod = 4;
00269                                    }
00270                             } else
00271                             {
00272                                    m_lastUsedMethod = 5;
00273                             }
00274                             
00275                      }
00276               }
00277        }
00278 
00279        if (isValid)
00280        {
00281 #ifdef __SPU__
00282               //spu_printf("distance\n");
00283 #endif //__CELLOS_LV2__
00284 
00285 
00286               output.addContactPoint(
00287                      normalInB,
00288                      pointOnB+positionOffset,
00289                      distance);
00290               //printf("gjk add:%f",distance);
00291        }
00292 
00293 
00294 }
00295 
00296 
00297 
00298 
00299