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