src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btMinkowskiPenetrationDepthSolver.h"
  17 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  18 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  19 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
  20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  21
  22 #define NUM_UNITSPHERE_POINTS 42
  23
  24
  25 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
  26                                                                                                    const btConvexShape* convexA,const btConvexShape* convexB,
  27                                                                                                    const btTransform& transA,const btTransform& transB,
  28                                                                                                    btVector3& v, btVector3& pa, btVector3& pb,
  29                                                                                                    class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
  30                                                                                                    )
  31 {
  32
  33         (void)stackAlloc;
  34         (void)v;
  35
  36         bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
  37
  38         struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
  39         {
  40
  41                 btIntermediateResult():m_hasResult(false)
  42                 {
  43                 }
  44
  45                 btVector3 m_normalOnBInWorld;
  46                 btVector3 m_pointInWorld;
  47                 btScalar m_depth;
  48                 bool    m_hasResult;
  49
  50                 virtual void setShapeIdentifiersA(int partId0,int index0)
  51                 {
  52                         (void)partId0;
  53                         (void)index0;
  54                 }
  55                 virtual void setShapeIdentifiersB(int partId1,int index1)
  56                 {
  57                         (void)partId1;
  58                         (void)index1;
  59                 }
  60                 void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
  61                 {
  62                         m_normalOnBInWorld = normalOnBInWorld;
  63                         m_pointInWorld = pointInWorld;
  64                         m_depth = depth;
  65                         m_hasResult = true;
  66                 }
  67         };
  68
  69         //just take fixed number of orientation, and sample the penetration depth in that direction
  70         btScalar minProj = btScalar(BT_LARGE_FLOAT);
  71         btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
  72         btVector3 minA,minB;
  73         btVector3 seperatingAxisInA,seperatingAxisInB;
  74         btVector3 pInA,qInB,pWorld,qWorld,w;
  75
  76 #ifndef __SPU__
  77 #define USE_BATCHED_SUPPORT 1
  78 #endif
  79 #ifdef USE_BATCHED_SUPPORT
  80
  81         btVector3       supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
  82         btVector3       supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
  83         btVector3       seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
  84         btVector3       seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
  85         int i;
  86
  87         int numSampleDirections = NUM_UNITSPHERE_POINTS;
  88
  89         for (i=0;i<numSampleDirections;i++)
  90         {
  91                 btVector3 norm = getPenetrationDirections()[i];
  92                 seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
  93                 seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
  94         }
  95
  96         {
  97                 int numPDA = convexA->getNumPreferredPenetrationDirections();
  98                 if (numPDA)
  99                 {
 100                         for (int i=0;i<numPDA;i++)
 101                         {
 102                                 btVector3 norm;
 103                                 convexA->getPreferredPenetrationDirection(i,norm);
 104                                 norm  = transA.getBasis() * norm;
 105                                 getPenetrationDirections()[numSampleDirections] = norm;
 106                                 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 107                                 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 108                                 numSampleDirections++;
 109                         }
 110                 }
 111         }
 112
 113         {
 114                 int numPDB = convexB->getNumPreferredPenetrationDirections();
 115                 if (numPDB)
 116                 {
 117                         for (int i=0;i<numPDB;i++)
 118                         {
 119                                 btVector3 norm;
 120                                 convexB->getPreferredPenetrationDirection(i,norm);
 121                                 norm  = transB.getBasis() * norm;
 122                                 getPenetrationDirections()[numSampleDirections] = norm;
 123                                 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 124                                 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 125                                 numSampleDirections++;
 126                         }
 127                 }
 128         }
 129
 130
 131
 132
 133         convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
 134         convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
 135
 136         for (i=0;i<numSampleDirections;i++)
 137         {
 138                 btVector3 norm = getPenetrationDirections()[i];
 139                 if (check2d)
 140                 {
 141                         norm[2] = 0.f;
 142                 }
 143                 if (norm.length2()>0.01)
 144                 {
 145
 146                         seperatingAxisInA = seperatingAxisInABatch[i];
 147                         seperatingAxisInB = seperatingAxisInBBatch[i];
 148
 149                         pInA = supportVerticesABatch[i];
 150                         qInB = supportVerticesBBatch[i];
 151
 152                         pWorld = transA(pInA);
 153                         qWorld = transB(qInB);
 154                         if (check2d)
 155                         {
 156                                 pWorld[2] = 0.f;
 157                                 qWorld[2] = 0.f;
 158                         }
 159
 160                         w       = qWorld - pWorld;
 161                         btScalar delta = norm.dot(w);
 162                         //find smallest delta
 163                         if (delta < minProj)
 164                         {
 165                                 minProj = delta;
 166                                 minNorm = norm;
 167                                 minA = pWorld;
 168                                 minB = qWorld;
 169                         }
 170                 }
 171         }
 172 #else
 173
 174         int numSampleDirections = NUM_UNITSPHERE_POINTS;
 175
 176 #ifndef __SPU__
 177         {
 178                 int numPDA = convexA->getNumPreferredPenetrationDirections();
 179                 if (numPDA)
 180                 {
 181                         for (int i=0;i<numPDA;i++)
 182                         {
 183                                 btVector3 norm;
 184                                 convexA->getPreferredPenetrationDirection(i,norm);
 185                                 norm  = transA.getBasis() * norm;
 186                                 getPenetrationDirections()[numSampleDirections] = norm;
 187                                 numSampleDirections++;
 188                         }
 189                 }
 190         }
 191
 192         {
 193                 int numPDB = convexB->getNumPreferredPenetrationDirections();
 194                 if (numPDB)
 195                 {
 196                         for (int i=0;i<numPDB;i++)
 197                         {
 198                                 btVector3 norm;
 199                                 convexB->getPreferredPenetrationDirection(i,norm);
 200                                 norm  = transB.getBasis() * norm;
 201                                 getPenetrationDirections()[numSampleDirections] = norm;
 202                                 numSampleDirections++;
 203                         }
 204                 }
 205         }
 206 #endif // __SPU__
 207
 208         for (int i=0;i<numSampleDirections;i++)
 209         {
 210                 const btVector3& norm = getPenetrationDirections()[i];
 211                 seperatingAxisInA = (-norm)* transA.getBasis();
 212                 seperatingAxisInB = norm* transB.getBasis();
 213                 pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 214                 qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 215                 pWorld = transA(pInA);
 216                 qWorld = transB(qInB);
 217                 w       = qWorld - pWorld;
 218                 btScalar delta = norm.dot(w);
 219                 //find smallest delta
 220                 if (delta < minProj)
 221                 {
 222                         minProj = delta;
 223                         minNorm = norm;
 224                         minA = pWorld;
 225                         minB = qWorld;
 226                 }
 227         }
 228 #endif //USE_BATCHED_SUPPORT
 229
 230         //add the margins
 231
 232         minA += minNorm*convexA->getMarginNonVirtual();
 233         minB -= minNorm*convexB->getMarginNonVirtual();
 234         //no penetration
 235         if (minProj < btScalar(0.))
 236                 return false;
 237
 238         btScalar extraSeparation = 0.5f;///scale dependent
 239         minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
 240
 241
 242
 243
 244
 245 //#define DEBUG_DRAW 1
 246 #ifdef DEBUG_DRAW
 247         if (debugDraw)
 248         {
 249                 btVector3 color(0,1,0);
 250                 debugDraw->drawLine(minA,minB,color);
 251                 color = btVector3 (1,1,1);
 252                 btVector3 vec = minB-minA;
 253                 btScalar prj2 = minNorm.dot(vec);
 254                 debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
 255
 256         }
 257 #endif //DEBUG_DRAW
 258
 259
 260
 261         btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
 262
 263         btScalar offsetDist = minProj;
 264         btVector3 offset = minNorm * offsetDist;
 265
 266
 267
 268         btGjkPairDetector::ClosestPointInput input;
 269
 270         btVector3 newOrg = transA.getOrigin() + offset;
 271
 272         btTransform displacedTrans = transA;
 273         displacedTrans.setOrigin(newOrg);
 274
 275         input.m_transformA = displacedTrans;
 276         input.m_transformB = transB;
 277         input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
 278
 279         btIntermediateResult res;
 280         gjkdet.setCachedSeperatingAxis(-minNorm);
 281         gjkdet.getClosestPoints(input,res,debugDraw);
 282
 283         btScalar correctedMinNorm = minProj - res.m_depth;
 284
 285
 286         //the penetration depth is over-estimated, relax it
 287         btScalar penetration_relaxation= btScalar(1.);
 288         minNorm*=penetration_relaxation;
 289
 290
 291         if (res.m_hasResult)
 292         {
 293
 294                 pa = res.m_pointInWorld - minNorm * correctedMinNorm;
 295                 pb = res.m_pointInWorld;
 296                 v = minNorm;
 297
 298 #ifdef DEBUG_DRAW
 299                 if (debugDraw)
 300                 {
 301                         btVector3 color(1,0,0);
 302                         debugDraw->drawLine(pa,pb,color);
 303                 }
 304 #endif//DEBUG_DRAW
 305
 306
 307         }
 308         return res.m_hasResult;
 309 }
 310
 311 btVector3*      btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
 312 {
 313         static btVector3        sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
 314         {
 315         btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
 316         btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
 317         btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
 318         btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
 319         btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
 320         btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
 321         btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
 322         btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
 323         btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
 324         btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
 325         btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
 326         btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
 327         btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
 328         btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
 329         btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
 330         btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
 331         btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
 332         btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
 333         btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
 334         btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
 335         btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
 336         btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
 337         btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
 338         btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
 339         btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
 340         btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
 341         btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
 342         btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
 343         btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
 344         btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
 345         btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
 346         btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
 347         btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
 348         btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
 349         btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
 350         btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
 351         btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
 352         btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
 353         btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
 354         btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
 355         btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
 356         btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
 357         };
 358
 359         return sPenetrationDirections;
 360 }
 361
 362