src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.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 "btGjkPairDetector.h"
  17 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  18 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
  19 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
  20
  21
  22
  23 #if defined(DEBUG) || defined (_DEBUG)
  24 //#define TEST_NON_VIRTUAL 1
  25 #include <stdio.h> //for debug printf
  26 #ifdef __SPU__
  27 #include <spu_printf.h>
  28 #define printf spu_printf
  29 //#define DEBUG_SPU_COLLISION_DETECTION 1
  30 #endif //__SPU__
  31 #endif
  32
  33 //must be above the machine epsilon
  34 #define REL_ERROR2 btScalar(1.0e-6)
  35
  36 //temp globals, to improve GJK/EPA/penetration calculations
  37 int gNumDeepPenetrationChecks = 0;
  38 int gNumGjkChecks = 0;
  39
  40
  41 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*  penetrationDepthSolver)
  42 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
  43 m_penetrationDepthSolver(penetrationDepthSolver),
  44 m_simplexSolver(simplexSolver),
  45 m_minkowskiA(objectA),
  46 m_minkowskiB(objectB),
  47 m_shapeTypeA(objectA->getShapeType()),
  48 m_shapeTypeB(objectB->getShapeType()),
  49 m_marginA(objectA->getMargin()),
  50 m_marginB(objectB->getMargin()),
  51 m_ignoreMargin(false),
  52 m_lastUsedMethod(-1),
  53 m_catchDegeneracies(1)
  54 {
  55 }
  56 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*        penetrationDepthSolver)
  57 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
  58 m_penetrationDepthSolver(penetrationDepthSolver),
  59 m_simplexSolver(simplexSolver),
  60 m_minkowskiA(objectA),
  61 m_minkowskiB(objectB),
  62 m_shapeTypeA(shapeTypeA),
  63 m_shapeTypeB(shapeTypeB),
  64 m_marginA(marginA),
  65 m_marginB(marginB),
  66 m_ignoreMargin(false),
  67 m_lastUsedMethod(-1),
  68 m_catchDegeneracies(1)
  69 {
  70 }
  71
  72 void    btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
  73 {
  74         (void)swapResults;
  75
  76         getClosestPointsNonVirtual(input,output,debugDraw);
  77 }
  78
  79 #ifdef __SPU__
  80 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
  81 #else
  82 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
  83 #endif
  84 {
  85         m_cachedSeparatingDistance = 0.f;
  86
  87         btScalar distance=btScalar(0.);
  88         btVector3       normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
  89         btVector3 pointOnA,pointOnB;
  90         btTransform     localTransA = input.m_transformA;
  91         btTransform localTransB = input.m_transformB;
  92         btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
  93         localTransA.getOrigin() -= positionOffset;
  94         localTransB.getOrigin() -= positionOffset;
  95
  96         bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
  97
  98         btScalar marginA = m_marginA;
  99         btScalar marginB = m_marginB;
 100
 101         gNumGjkChecks++;
 102
 103 #ifdef DEBUG_SPU_COLLISION_DETECTION
 104         spu_printf("inside gjk\n");
 105 #endif
 106         //for CCD we don't use margins
 107         if (m_ignoreMargin)
 108         {
 109                 marginA = btScalar(0.);
 110                 marginB = btScalar(0.);
 111 #ifdef DEBUG_SPU_COLLISION_DETECTION
 112                 spu_printf("ignoring margin\n");
 113 #endif
 114         }
 115
 116         m_curIter = 0;
 117         int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
 118         m_cachedSeparatingAxis.setValue(0,1,0);
 119
 120         bool isValid = false;
 121         bool checkSimplex = false;
 122         bool checkPenetration = true;
 123         m_degenerateSimplex = 0;
 124
 125         m_lastUsedMethod = -1;
 126
 127         {
 128                 btScalar squaredDistance = BT_LARGE_FLOAT;
 129                 btScalar delta = btScalar(0.);
 130
 131                 btScalar margin = marginA + marginB;
 132
 133
 134
 135                 m_simplexSolver->reset();
 136
 137                 for ( ; ; )
 138                 //while (true)
 139                 {
 140
 141                         btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
 142                         btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
 143
 144 #if 1
 145
 146                         btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 147                         btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 148
 149 //                      btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
 150 //                      btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
 151
 152 #else
 153 #ifdef __SPU__
 154                         btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 155                         btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 156 #else
 157                         btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
 158                         btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
 159 #ifdef TEST_NON_VIRTUAL
 160                         btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
 161                         btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
 162                         btAssert((pInAv-pInA).length() < 0.0001);
 163                         btAssert((qInBv-qInB).length() < 0.0001);
 164 #endif //
 165 #endif //__SPU__
 166 #endif
 167
 168
 169                         btVector3  pWorld = localTransA(pInA);
 170                         btVector3  qWorld = localTransB(qInB);
 171
 172 #ifdef DEBUG_SPU_COLLISION_DETECTION
 173                 spu_printf("got local supporting vertices\n");
 174 #endif
 175
 176                         if (check2d)
 177                         {
 178                                 pWorld[2] = 0.f;
 179                                 qWorld[2] = 0.f;
 180                         }
 181
 182                         btVector3 w     = pWorld - qWorld;
 183                         delta = m_cachedSeparatingAxis.dot(w);
 184
 185                         // potential exit, they don't overlap
 186                         if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
 187                         {
 188                                 m_degenerateSimplex = 10;
 189                                 checkSimplex=true;
 190                                 //checkPenetration = false;
 191                                 break;
 192                         }
 193
 194                         //exit 0: the new point is already in the simplex, or we didn't come any closer
 195                         if (m_simplexSolver->inSimplex(w))
 196                         {
 197                                 m_degenerateSimplex = 1;
 198                                 checkSimplex = true;
 199                                 break;
 200                         }
 201                         // are we getting any closer ?
 202                         btScalar f0 = squaredDistance - delta;
 203                         btScalar f1 = squaredDistance * REL_ERROR2;
 204
 205                         if (f0 <= f1)
 206                         {
 207                                 if (f0 <= btScalar(0.))
 208                                 {
 209                                         m_degenerateSimplex = 2;
 210                                 } else
 211                                 {
 212                                         m_degenerateSimplex = 11;
 213                                 }
 214                                 checkSimplex = true;
 215                                 break;
 216                         }
 217
 218 #ifdef DEBUG_SPU_COLLISION_DETECTION
 219                 spu_printf("addVertex 1\n");
 220 #endif
 221                         //add current vertex to simplex
 222                         m_simplexSolver->addVertex(w, pWorld, qWorld);
 223 #ifdef DEBUG_SPU_COLLISION_DETECTION
 224                 spu_printf("addVertex 2\n");
 225 #endif
 226                         btVector3 newCachedSeparatingAxis;
 227
 228                         //calculate the closest point to the origin (update vector v)
 229                         if (!m_simplexSolver->closest(newCachedSeparatingAxis))
 230                         {
 231                                 m_degenerateSimplex = 3;
 232                                 checkSimplex = true;
 233                                 break;
 234                         }
 235
 236                         if(newCachedSeparatingAxis.length2()<REL_ERROR2)
 237             {
 238                                 m_cachedSeparatingAxis = newCachedSeparatingAxis;
 239                 m_degenerateSimplex = 6;
 240                 checkSimplex = true;
 241                 break;
 242             }
 243
 244                         btScalar previousSquaredDistance = squaredDistance;
 245                         squaredDistance = newCachedSeparatingAxis.length2();
 246 #if 0
 247 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
 248                         if (squaredDistance>previousSquaredDistance)
 249                         {
 250                                 m_degenerateSimplex = 7;
 251                                 squaredDistance = previousSquaredDistance;
 252                 checkSimplex = false;
 253                 break;
 254                         }
 255 #endif //
 256
 257
 258                         //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
 259
 260                         //are we getting any closer ?
 261                         if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
 262                         {
 263 //                              m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
 264                                 checkSimplex = true;
 265                                 m_degenerateSimplex = 12;
 266
 267                                 break;
 268                         }
 269
 270                         m_cachedSeparatingAxis = newCachedSeparatingAxis;
 271
 272                           //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
 273               if (m_curIter++ > gGjkMaxIter)
 274               {
 275                       #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
 276
 277                               printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
 278                               printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
 279                               m_cachedSeparatingAxis.getX(),
 280                               m_cachedSeparatingAxis.getY(),
 281                               m_cachedSeparatingAxis.getZ(),
 282                               squaredDistance,
 283                               m_minkowskiA->getShapeType(),
 284                               m_minkowskiB->getShapeType());
 285
 286                       #endif
 287                       break;
 288
 289               }
 290
 291
 292                         bool check = (!m_simplexSolver->fullSimplex());
 293                         //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
 294
 295                         if (!check)
 296                         {
 297                                 //do we need this backup_closest here ?
 298 //                              m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
 299                                 m_degenerateSimplex = 13;
 300                                 break;
 301                         }
 302                 }
 303
 304                 if (checkSimplex)
 305                 {
 306                         m_simplexSolver->compute_points(pointOnA, pointOnB);
 307                         normalInB = m_cachedSeparatingAxis;
 308                         btScalar lenSqr =m_cachedSeparatingAxis.length2();
 309
 310                         //valid normal
 311                         if (lenSqr < 0.0001)
 312                         {
 313                                 m_degenerateSimplex = 5;
 314                         }
 315                         if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
 316                         {
 317                                 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
 318                                 normalInB *= rlen; //normalize
 319                                 btScalar s = btSqrt(squaredDistance);
 320
 321                                 btAssert(s > btScalar(0.0));
 322                                 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
 323                                 pointOnB += m_cachedSeparatingAxis * (marginB / s);
 324                                 distance = ((btScalar(1.)/rlen) - margin);
 325                                 isValid = true;
 326
 327                                 m_lastUsedMethod = 1;
 328                         } else
 329                         {
 330                                 m_lastUsedMethod = 2;
 331                         }
 332                 }
 333
 334                 bool catchDegeneratePenetrationCase =
 335                         (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
 336
 337                 //if (checkPenetration && !isValid)
 338                 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
 339                 {
 340                         //penetration case
 341
 342                         //if there is no way to handle penetrations, bail out
 343                         if (m_penetrationDepthSolver)
 344                         {
 345                                 // Penetration depth case.
 346                                 btVector3 tmpPointOnA,tmpPointOnB;
 347
 348                                 gNumDeepPenetrationChecks++;
 349                                 m_cachedSeparatingAxis.setZero();
 350
 351                                 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
 352                                         *m_simplexSolver,
 353                                         m_minkowskiA,m_minkowskiB,
 354                                         localTransA,localTransB,
 355                                         m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
 356                                         debugDraw,input.m_stackAlloc
 357                                         );
 358
 359
 360                                 if (isValid2)
 361                                 {
 362                                         btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
 363                                         btScalar lenSqr = tmpNormalInB.length2();
 364                                         if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
 365                                         {
 366                                                 tmpNormalInB = m_cachedSeparatingAxis;
 367                                                 lenSqr = m_cachedSeparatingAxis.length2();
 368                                         }
 369
 370                                         if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
 371                                         {
 372                                                 tmpNormalInB /= btSqrt(lenSqr);
 373                                                 btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
 374                                                 //only replace valid penetrations when the result is deeper (check)
 375                                                 if (!isValid || (distance2 < distance))
 376                                                 {
 377                                                         distance = distance2;
 378                                                         pointOnA = tmpPointOnA;
 379                                                         pointOnB = tmpPointOnB;
 380                                                         normalInB = tmpNormalInB;
 381                                                         isValid = true;
 382                                                         m_lastUsedMethod = 3;
 383                                                 } else
 384                                                 {
 385                                                         m_lastUsedMethod = 8;
 386                                                 }
 387                                         } else
 388                                         {
 389                                                 m_lastUsedMethod = 9;
 390                                         }
 391                                 } else
 392
 393                                 {
 394                                         ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
 395                                         ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
 396                                         ///reports a valid positive distance. Use the results of the second GJK instead of failing.
 397                                         ///thanks to Jacob.Langford for the reproduction case
 398                                         ///http://code.google.com/p/bullet/issues/detail?id=250
 399
 400
 401                                         if (m_cachedSeparatingAxis.length2() > btScalar(0.))
 402                                         {
 403                                                 btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
 404                                                 //only replace valid distances when the distance is less
 405                                                 if (!isValid || (distance2 < distance))
 406                                                 {
 407                                                         distance = distance2;
 408                                                         pointOnA = tmpPointOnA;
 409                                                         pointOnB = tmpPointOnB;
 410                                                         pointOnA -= m_cachedSeparatingAxis * marginA ;
 411                                                         pointOnB += m_cachedSeparatingAxis * marginB ;
 412                                                         normalInB = m_cachedSeparatingAxis;
 413                                                         normalInB.normalize();
 414                                                         isValid = true;
 415                                                         m_lastUsedMethod = 6;
 416                                                 } else
 417                                                 {
 418                                                         m_lastUsedMethod = 5;
 419                                                 }
 420                                         }
 421                                 }
 422
 423                         }
 424
 425                 }
 426         }
 427
 428
 429
 430         if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
 431         {
 432 #if 0
 433 ///some debugging
 434 //              if (check2d)
 435                 {
 436                         printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
 437                         printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
 438                 }
 439 #endif
 440
 441                 m_cachedSeparatingAxis = normalInB;
 442                 m_cachedSeparatingDistance = distance;
 443
 444                 output.addContactPoint(
 445                         normalInB,
 446                         pointOnB+positionOffset,
 447                         distance);
 448
 449         }
 450
 451
 452 }
 453
 454
 455
 456
 457