src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.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 "SpuMinkowskiPenetrationDepthSolver.h"
  17 #include "SpuContactResult.h"
  18 #include "SpuPreferredPenetrationDirections.h"
  19 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  20 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
  21 #include "SpuCollisionShapes.h"
  22
  23 #define NUM_UNITSPHERE_POINTS 42
  24 static btVector3        sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
  25 {
  26 btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
  27 btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
  28 btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
  29 btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
  30 btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
  31 btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
  32 btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
  33 btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
  34 btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
  35 btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
  36 btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
  37 btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
  38 btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
  39 btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
  40 btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
  41 btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
  42 btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
  43 btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
  44 btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
  45 btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
  46 btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
  47 btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
  48 btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
  49 btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
  50 btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
  51 btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
  52 btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
  53 btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
  54 btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
  55 btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
  56 btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
  57 btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
  58 btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
  59 btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
  60 btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
  61 btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
  62 btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
  63 btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
  64 btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
  65 btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
  66 btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
  67 btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
  68 };
  69
  70
  71 bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
  72                 const btConvexShape* convexA,const btConvexShape* convexB,
  73                                         const btTransform& transA,const btTransform& transB,
  74                                 btVector3& v, btVector3& pa, btVector3& pb,
  75                                 class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc)
  76 {
  77 #if 0
  78         (void)stackAlloc;
  79         (void)v;
  80
  81
  82         struct btIntermediateResult : public SpuContactResult
  83         {
  84
  85                 btIntermediateResult():m_hasResult(false)
  86                 {
  87                 }
  88
  89                 btVector3 m_normalOnBInWorld;
  90                 btVector3 m_pointInWorld;
  91                 btScalar m_depth;
  92                 bool    m_hasResult;
  93
  94                 virtual void setShapeIdentifiersA(int partId0,int index0)
  95                 {
  96                         (void)partId0;
  97                         (void)index0;
  98                 }
  99
 100                 virtual void setShapeIdentifiersB(int partId1,int index1)
 101                 {
 102                         (void)partId1;
 103                         (void)index1;
 104                 }
 105                 void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
 106                 {
 107                         m_normalOnBInWorld = normalOnBInWorld;
 108                         m_pointInWorld = pointInWorld;
 109                         m_depth = depth;
 110                         m_hasResult = true;
 111                 }
 112         };
 113
 114         //just take fixed number of orientation, and sample the penetration depth in that direction
 115         btScalar minProj = btScalar(BT_LARGE_FLOAT);
 116         btVector3 minNorm(0.f,0.f,0.f);
 117         btVector3 minVertex;
 118         btVector3 minA,minB;
 119         btVector3 seperatingAxisInA,seperatingAxisInB;
 120         btVector3 pInA,qInB,pWorld,qWorld,w;
 121
 122 //#define USE_BATCHED_SUPPORT 1
 123 #ifdef USE_BATCHED_SUPPORT
 124
 125         btVector3       supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 126         btVector3       supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 127         btVector3       seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 128         btVector3       seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 129         int i;
 130
 131         int numSampleDirections = NUM_UNITSPHERE_POINTS;
 132
 133         for (i=0;i<numSampleDirections;i++)
 134         {
 135                 const btVector3& norm = sPenetrationDirections[i];
 136                 seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
 137                 seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
 138         }
 139
 140         {
 141                 int numPDA = convexA->getNumPreferredPenetrationDirections();
 142                 if (numPDA)
 143                 {
 144                         for (int i=0;i<numPDA;i++)
 145                         {
 146                                 btVector3 norm;
 147                                 convexA->getPreferredPenetrationDirection(i,norm);
 148                                 norm  = transA.getBasis() * norm;
 149                                 sPenetrationDirections[numSampleDirections] = norm;
 150                                 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 151                                 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 152                                 numSampleDirections++;
 153                         }
 154                 }
 155         }
 156
 157         {
 158                 int numPDB = convexB->getNumPreferredPenetrationDirections();
 159                 if (numPDB)
 160                 {
 161                         for (int i=0;i<numPDB;i++)
 162                         {
 163                                 btVector3 norm;
 164                                 convexB->getPreferredPenetrationDirection(i,norm);
 165                                 norm  = transB.getBasis() * norm;
 166                                 sPenetrationDirections[numSampleDirections] = norm;
 167                                 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 168                                 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 169                                 numSampleDirections++;
 170                         }
 171                 }
 172         }
 173
 174
 175
 176         convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
 177         convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
 178
 179         for (i=0;i<numSampleDirections;i++)
 180         {
 181                 const btVector3& norm = sPenetrationDirections[i];
 182                 seperatingAxisInA = seperatingAxisInABatch[i];
 183                 seperatingAxisInB = seperatingAxisInBBatch[i];
 184
 185                 pInA = supportVerticesABatch[i];
 186                 qInB = supportVerticesBBatch[i];
 187
 188                 pWorld = transA(pInA);
 189                 qWorld = transB(qInB);
 190                 w       = qWorld - pWorld;
 191                 btScalar delta = norm.dot(w);
 192                 //find smallest delta
 193                 if (delta < minProj)
 194                 {
 195                         minProj = delta;
 196                         minNorm = norm;
 197                         minA = pWorld;
 198                         minB = qWorld;
 199                 }
 200         }
 201 #else
 202
 203         int numSampleDirections = NUM_UNITSPHERE_POINTS;
 204
 205 ///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh
 206 #define DO_PREFERRED_DIRECTIONS 1
 207 #ifdef DO_PREFERRED_DIRECTIONS
 208         {
 209                 int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA);
 210                 if (numPDA)
 211                 {
 212                         for (int i=0;i<numPDA;i++)
 213                         {
 214                                 btVector3 norm;
 215                                 spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm);
 216                                 norm  = transA.getBasis() * norm;
 217                                 sPenetrationDirections[numSampleDirections] = norm;
 218                                 numSampleDirections++;
 219                         }
 220                 }
 221         }
 222
 223         {
 224                 int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB);
 225                 if (numPDB)
 226                 {
 227                         for (int i=0;i<numPDB;i++)
 228                         {
 229                                 btVector3 norm;
 230                                 spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm);
 231                                 norm  = transB.getBasis() * norm;
 232                                 sPenetrationDirections[numSampleDirections] = norm;
 233                                 numSampleDirections++;
 234                         }
 235                 }
 236         }
 237 #endif //DO_PREFERRED_DIRECTIONS
 238
 239         for (int i=0;i<numSampleDirections;i++)
 240         {
 241                 const btVector3& norm = sPenetrationDirections[i];
 242                 seperatingAxisInA = (-norm)* transA.getBasis();
 243                 seperatingAxisInB = norm* transB.getBasis();
 244
 245                 pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual( seperatingAxisInA);//, NULL);
 246                 qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);//, NULL);
 247
 248         //      pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
 249         //      qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
 250
 251                 pWorld = transA(pInA);
 252                 qWorld = transB(qInB);
 253                 w       = qWorld - pWorld;
 254                 btScalar delta = norm.dot(w);
 255                 //find smallest delta
 256                 if (delta < minProj)
 257                 {
 258                         minProj = delta;
 259                         minNorm = norm;
 260                         minA = pWorld;
 261                         minB = qWorld;
 262                 }
 263         }
 264 #endif //USE_BATCHED_SUPPORT
 265
 266         //add the margins
 267
 268         minA += minNorm*marginA;
 269         minB -= minNorm*marginB;
 270         //no penetration
 271         if (minProj < btScalar(0.))
 272                 return false;
 273
 274         minProj += (marginA + marginB) + btScalar(1.00);
 275
 276
 277
 278
 279
 280 //#define DEBUG_DRAW 1
 281 #ifdef DEBUG_DRAW
 282         if (debugDraw)
 283         {
 284                 btVector3 color(0,1,0);
 285                 debugDraw->drawLine(minA,minB,color);
 286                 color = btVector3 (1,1,1);
 287                 btVector3 vec = minB-minA;
 288                 btScalar prj2 = minNorm.dot(vec);
 289                 debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
 290
 291         }
 292 #endif //DEBUG_DRAW
 293
 294
 295         btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
 296
 297         btScalar offsetDist = minProj;
 298         btVector3 offset = minNorm * offsetDist;
 299
 300
 301         SpuClosestPointInput input;
 302         input.m_convexVertexData[0] = convexVertexDataA;
 303         input.m_convexVertexData[1] = convexVertexDataB;
 304         btVector3 newOrg = transA.getOrigin() + offset;
 305
 306         btTransform displacedTrans = transA;
 307         displacedTrans.setOrigin(newOrg);
 308
 309         input.m_transformA = displacedTrans;
 310         input.m_transformB = transB;
 311         input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
 312
 313         btIntermediateResult res;
 314         gjkdet.getClosestPoints(input,res,0);
 315
 316         btScalar correctedMinNorm = minProj - res.m_depth;
 317
 318
 319         //the penetration depth is over-estimated, relax it
 320         btScalar penetration_relaxation= btScalar(1.);
 321         minNorm*=penetration_relaxation;
 322
 323         if (res.m_hasResult)
 324         {
 325
 326                 pa = res.m_pointInWorld - minNorm * correctedMinNorm;
 327                 pb = res.m_pointInWorld;
 328
 329 #ifdef DEBUG_DRAW
 330                 if (debugDraw)
 331                 {
 332                         btVector3 color(1,0,0);
 333                         debugDraw->drawLine(pa,pb,color);
 334                 }
 335 #endif//DEBUG_DRAW
 336
 337
 338         } else {
 339                 // could not seperate shapes
 340                 //btAssert (false);
 341         }
 342         return res.m_hasResult;
 343 #endif
 344         return false;
 345 }
 346
 347
 348