Back to index

supertuxkart  0.5+dfsg1
btMinkowskiPenetrationDepthSolver.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 "btMinkowskiPenetrationDepthSolver.h"
00017 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00018 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
00019 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
00020 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00021 
00022 
00023 
00024 #define NUM_UNITSPHERE_POINTS 42
00025 static btVector3     sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
00026 {
00027 btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
00028 btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
00029 btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
00030 btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
00031 btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
00032 btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
00033 btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
00034 btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
00035 btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
00036 btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
00037 btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
00038 btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
00039 btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
00040 btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
00041 btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
00042 btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
00043 btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
00044 btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
00045 btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
00046 btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
00047 btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
00048 btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
00049 btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
00050 btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
00051 btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
00052 btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
00053 btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
00054 btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
00055 btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
00056 btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
00057 btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
00058 btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
00059 btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
00060 btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
00061 btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
00062 btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
00063 btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
00064 btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
00065 btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
00066 btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
00067 btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
00068 btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
00069 };
00070 
00071 
00072 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
00073                                                                                        const btConvexShape* convexA,const btConvexShape* convexB,
00074                                                                                        const btTransform& transA,const btTransform& transB,
00075                                                                                        btVector3& v, btPoint3& pa, btPoint3& pb,
00076                                                                                        class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
00077                                                                                        )
00078 {
00079 
00080        (void)stackAlloc;
00081        (void)v;
00082        
00083 
00084        struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
00085        {
00086 
00087               btIntermediateResult():m_hasResult(false)
00088               {
00089               }
00090               
00091               btVector3 m_normalOnBInWorld;
00092               btVector3 m_pointInWorld;
00093               btScalar m_depth;
00094               bool   m_hasResult;
00095 
00096               virtual void setShapeIdentifiers(int partId0,int index0,       int partId1,int index1)
00097               {
00098                      (void)partId0;
00099                      (void)index0;
00100                      (void)partId1;
00101                      (void)index1;
00102               }
00103               void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
00104               {
00105                      m_normalOnBInWorld = normalOnBInWorld;
00106                      m_pointInWorld = pointInWorld;
00107                      m_depth = depth;
00108                      m_hasResult = true;
00109               }
00110        };
00111 
00112        //just take fixed number of orientation, and sample the penetration depth in that direction
00113        btScalar minProj = btScalar(1e30);
00114        btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
00115        btVector3 minA,minB;
00116        btVector3 seperatingAxisInA,seperatingAxisInB;
00117        btVector3 pInA,qInB,pWorld,qWorld,w;
00118 
00119 #define USE_BATCHED_SUPPORT 1
00120 #ifdef USE_BATCHED_SUPPORT
00121 
00122        btVector3     supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
00123        btVector3     supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
00124        btVector3     seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
00125        btVector3     seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
00126        int i;
00127 
00128        int numSampleDirections = NUM_UNITSPHERE_POINTS;
00129 
00130        for (i=0;i<numSampleDirections;i++)
00131        {
00132               const btVector3& norm = sPenetrationDirections[i];
00133               seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
00134               seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
00135        }
00136 
00137        {
00138               int numPDA = convexA->getNumPreferredPenetrationDirections();
00139               if (numPDA)
00140               {
00141                      for (int i=0;i<numPDA;i++)
00142                      {
00143                             btVector3 norm;
00144                             convexA->getPreferredPenetrationDirection(i,norm);
00145                             norm  = transA.getBasis() * norm;
00146                             sPenetrationDirections[numSampleDirections] = norm;
00147                             seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
00148                             seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
00149                             numSampleDirections++;
00150                      }
00151               }
00152        }
00153 
00154        {
00155               int numPDB = convexB->getNumPreferredPenetrationDirections();
00156               if (numPDB)
00157               {
00158                      for (int i=0;i<numPDB;i++)
00159                      {
00160                             btVector3 norm;
00161                             convexB->getPreferredPenetrationDirection(i,norm);
00162                             norm  = transB.getBasis() * norm;
00163                             sPenetrationDirections[numSampleDirections] = norm;
00164                             seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
00165                             seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
00166                             numSampleDirections++;
00167                      }
00168               }
00169        }
00170 
00171 
00172 
00173        convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
00174        convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
00175 
00176        for (i=0;i<numSampleDirections;i++)
00177        {
00178               const btVector3& norm = sPenetrationDirections[i];
00179               seperatingAxisInA = seperatingAxisInABatch[i];
00180               seperatingAxisInB = seperatingAxisInBBatch[i];
00181 
00182               pInA = supportVerticesABatch[i];
00183               qInB = supportVerticesBBatch[i];
00184 
00185               pWorld = transA(pInA);      
00186               qWorld = transB(qInB);
00187               w      = qWorld - pWorld;
00188               btScalar delta = norm.dot(w);
00189               //find smallest delta
00190               if (delta < minProj)
00191               {
00192                      minProj = delta;
00193                      minNorm = norm;
00194                      minA = pWorld;
00195                      minB = qWorld;
00196               }
00197        }      
00198 #else
00199 
00200        int numSampleDirections = NUM_UNITSPHERE_POINTS;
00201 
00202        {
00203               int numPDA = convexA->getNumPreferredPenetrationDirections();
00204               if (numPDA)
00205               {
00206                      for (int i=0;i<numPDA;i++)
00207                      {
00208                             btVector3 norm;
00209                             convexA->getPreferredPenetrationDirection(i,norm);
00210                             norm  = transA.getBasis() * norm;
00211                             sPenetrationDirections[numSampleDirections] = norm;
00212                             numSampleDirections++;
00213                      }
00214               }
00215        }
00216 
00217        {
00218               int numPDB = convexB->getNumPreferredPenetrationDirections();
00219               if (numPDB)
00220               {
00221                      for (int i=0;i<numPDB;i++)
00222                      {
00223                             btVector3 norm;
00224                             convexB->getPreferredPenetrationDirection(i,norm);
00225                             norm  = transB.getBasis() * norm;
00226                             sPenetrationDirections[numSampleDirections] = norm;
00227                             numSampleDirections++;
00228                      }
00229               }
00230        }
00231 
00232        for (int i=0;i<numSampleDirections;i++)
00233        {
00234               const btVector3& norm = sPenetrationDirections[i];
00235               seperatingAxisInA = (-norm)* transA.getBasis();
00236               seperatingAxisInB = norm* transB.getBasis();
00237               pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
00238               qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
00239               pWorld = transA(pInA);      
00240               qWorld = transB(qInB);
00241               w      = qWorld - pWorld;
00242               btScalar delta = norm.dot(w);
00243               //find smallest delta
00244               if (delta < minProj)
00245               {
00246                      minProj = delta;
00247                      minNorm = norm;
00248                      minA = pWorld;
00249                      minB = qWorld;
00250               }
00251        }
00252 #endif //USE_BATCHED_SUPPORT
00253 
00254        //add the margins
00255 
00256        minA += minNorm*convexA->getMargin();
00257        minB -= minNorm*convexB->getMargin();
00258        //no penetration
00259        if (minProj < btScalar(0.))
00260               return false;
00261 
00262        minProj += (convexA->getMargin() + convexB->getMargin());
00263 
00264 
00265 
00266 
00267 
00268 //#define DEBUG_DRAW 1
00269 #ifdef DEBUG_DRAW
00270        if (debugDraw)
00271        {
00272               btVector3 color(0,1,0);
00273               debugDraw->drawLine(minA,minB,color);
00274               color = btVector3 (1,1,1);
00275               btVector3 vec = minB-minA;
00276               btScalar prj2 = minNorm.dot(vec);
00277               debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
00278 
00279        }
00280 #endif //DEBUG_DRAW
00281 
00282        
00283 
00284        btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
00285 
00286        btScalar offsetDist = minProj;
00287        btVector3 offset = minNorm * offsetDist;
00288        
00289 
00290 
00291        btGjkPairDetector::ClosestPointInput input;
00292               
00293        btVector3 newOrg = transA.getOrigin() + offset;
00294 
00295        btTransform displacedTrans = transA;
00296        displacedTrans.setOrigin(newOrg);
00297 
00298        input.m_transformA = displacedTrans;
00299        input.m_transformB = transB;
00300        input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
00301        
00302        btIntermediateResult res;
00303        gjkdet.getClosestPoints(input,res,debugDraw);
00304 
00305        btScalar correctedMinNorm = minProj - res.m_depth;
00306 
00307 
00308        //the penetration depth is over-estimated, relax it
00309        btScalar penetration_relaxation= btScalar(1.);
00310        minNorm*=penetration_relaxation;
00311 
00312        if (res.m_hasResult)
00313        {
00314 
00315               pa = res.m_pointInWorld - minNorm * correctedMinNorm;
00316               pb = res.m_pointInWorld;
00317               
00318 #ifdef DEBUG_DRAW
00319               if (debugDraw)
00320               {
00321                      btVector3 color(1,0,0);
00322                      debugDraw->drawLine(pa,pb,color);
00323               }
00324 #endif//DEBUG_DRAW
00325 
00326 
00327        }
00328        return res.m_hasResult;
00329 }
00330 
00331 
00332