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