dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
   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
  17 #include "btCompoundCompoundCollisionAlgorithm.h"
  18 #include "LinearMath/btQuickprof.h"
  19 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  20 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
  21 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
  22 #include "LinearMath/btIDebugDraw.h"
  23 #include "LinearMath/btAabbUtil2.h"
  24 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
  25 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  26
  27 //USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT
  28 #define USE_LOCAL_STACK 1
  29
  30 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
  31
  32 btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
  33         : btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, isSwapped)
  34 {
  35         void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache), 16);
  36         m_childCollisionAlgorithmCache = new (ptr) btHashedSimplePairCache();
  37
  38         const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
  39         btAssert(col0ObjWrap->getCollisionShape()->isCompound());
  40
  41         const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
  42         btAssert(col1ObjWrap->getCollisionShape()->isCompound());
  43
  44         const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
  45         m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
  46
  47         const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
  48         m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
  49 }
  50
  51 btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
  52 {
  53         removeChildAlgorithms();
  54         m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
  55         btAlignedFree(m_childCollisionAlgorithmCache);
  56 }
  57
  58 void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
  59 {
  60         int i;
  61         btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
  62         for (i = 0; i < pairs.size(); i++)
  63         {
  64                 if (pairs[i].m_userPointer)
  65                 {
  66                         ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
  67                 }
  68         }
  69 }
  70
  71 void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
  72 {
  73         btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
  74
  75         int numChildren = pairs.size();
  76         int i;
  77         for (i = 0; i < numChildren; i++)
  78         {
  79                 if (pairs[i].m_userPointer)
  80                 {
  81                         btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
  82                         algo->~btCollisionAlgorithm();
  83                         m_dispatcher->freeCollisionAlgorithm(algo);
  84                 }
  85         }
  86         m_childCollisionAlgorithmCache->removeAllPairs();
  87 }
  88
  89 struct btCompoundCompoundLeafCallback : btDbvt::ICollide
  90 {
  91         int m_numOverlapPairs;
  92
  93         const btCollisionObjectWrapper* m_compound0ColObjWrap;
  94         const btCollisionObjectWrapper* m_compound1ColObjWrap;
  95         btDispatcher* m_dispatcher;
  96         const btDispatcherInfo& m_dispatchInfo;
  97         btManifoldResult* m_resultOut;
  98
  99         class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
 100
 101         btPersistentManifold* m_sharedManifold;
 102
 103         btCompoundCompoundLeafCallback(const btCollisionObjectWrapper* compound1ObjWrap,
 104                                                                    const btCollisionObjectWrapper* compound0ObjWrap,
 105                                                                    btDispatcher* dispatcher,
 106                                                                    const btDispatcherInfo& dispatchInfo,
 107                                                                    btManifoldResult* resultOut,
 108                                                                    btHashedSimplePairCache* childAlgorithmsCache,
 109                                                                    btPersistentManifold* sharedManifold)
 110                 : m_numOverlapPairs(0), m_compound0ColObjWrap(compound1ObjWrap), m_compound1ColObjWrap(compound0ObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithmCache(childAlgorithmsCache), m_sharedManifold(sharedManifold)
 111         {
 112         }
 113
 114         void Process(const btDbvtNode* leaf0, const btDbvtNode* leaf1)
 115         {
 116                 BT_PROFILE("btCompoundCompoundLeafCallback::Process");
 117                 m_numOverlapPairs++;
 118
 119                 int childIndex0 = leaf0->dataAsInt;
 120                 int childIndex1 = leaf1->dataAsInt;
 121
 122                 btAssert(childIndex0 >= 0);
 123                 btAssert(childIndex1 >= 0);
 124
 125                 const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
 126                 btAssert(childIndex0 < compoundShape0->getNumChildShapes());
 127
 128                 const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
 129                 btAssert(childIndex1 < compoundShape1->getNumChildShapes());
 130
 131                 const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
 132                 const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
 133
 134                 //backup
 135                 btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
 136                 const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
 137                 btTransform newChildWorldTrans0 = orgTrans0 * childTrans0;
 138
 139                 btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
 140                 const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
 141                 btTransform newChildWorldTrans1 = orgTrans1 * childTrans1;
 142
 143                 //perform an AABB check first
 144                 btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
 145                 childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0);
 146                 childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1);
 147
 148                 btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
 149
 150                 aabbMin0 -= thresholdVec;
 151                 aabbMax0 += thresholdVec;
 152
 153                 if (gCompoundCompoundChildShapePairCallback)
 154                 {
 155                         if (!gCompoundCompoundChildShapePairCallback(childShape0, childShape1))
 156                                 return;
 157                 }
 158
 159                 if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 160                 {
 161                         btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap, childShape0, m_compound0ColObjWrap->getCollisionObject(), newChildWorldTrans0, -1, childIndex0);
 162                         btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap, childShape1, m_compound1ColObjWrap->getCollisionObject(), newChildWorldTrans1, -1, childIndex1);
 163
 164                         btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0, childIndex1);
 165                         bool removePair = false;
 166                         btCollisionAlgorithm* colAlgo = 0;
 167                         if (m_resultOut->m_closestPointDistanceThreshold > 0)
 168                         {
 169                                 colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
 170                                 removePair = true;
 171                         }
 172                         else
 173                         {
 174                                 if (pair)
 175                                 {
 176                                         colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
 177                                 }
 178                                 else
 179                                 {
 180                                         colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
 181                                         pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
 182                                         btAssert(pair);
 183                                         pair->m_userPointer = colAlgo;
 184                                 }
 185                         }
 186
 187                         btAssert(colAlgo);
 188
 189                         const btCollisionObjectWrapper* tmpWrap0 = 0;
 190                         const btCollisionObjectWrapper* tmpWrap1 = 0;
 191
 192                         tmpWrap0 = m_resultOut->getBody0Wrap();
 193                         tmpWrap1 = m_resultOut->getBody1Wrap();
 194
 195                         m_resultOut->setBody0Wrap(&compoundWrap0);
 196                         m_resultOut->setBody1Wrap(&compoundWrap1);
 197
 198                         m_resultOut->setShapeIdentifiersA(-1, childIndex0);
 199                         m_resultOut->setShapeIdentifiersB(-1, childIndex1);
 200
 201                         colAlgo->processCollision(&compoundWrap0, &compoundWrap1, m_dispatchInfo, m_resultOut);
 202
 203                         m_resultOut->setBody0Wrap(tmpWrap0);
 204                         m_resultOut->setBody1Wrap(tmpWrap1);
 205
 206                         if (removePair)
 207                         {
 208                                 colAlgo->~btCollisionAlgorithm();
 209                                 m_dispatcher->freeCollisionAlgorithm(colAlgo);
 210                         }
 211                 }
 212         }
 213 };
 214
 215 static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm& a,
 216                                                                         const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
 217 {
 218         btVector3 newmin, newmax;
 219         btTransformAabb(b.Mins(), b.Maxs(), 0.f, xform, newmin, newmax);
 220         newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
 221         newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
 222         btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin, newmax);
 223         return Intersect(a, newb);
 224 }
 225
 226 static inline void MycollideTT(const btDbvtNode* root0,
 227                                                            const btDbvtNode* root1,
 228                                                            const btTransform& xform,
 229                                                            btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
 230 {
 231         if (root0 && root1)
 232         {
 233                 int depth = 1;
 234                 int treshold = btDbvt::DOUBLE_STACKSIZE - 4;
 235                 btAlignedObjectArray<btDbvt::sStkNN> stkStack;
 236 #ifdef USE_LOCAL_STACK
 237                 ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]);
 238                 stkStack.initializeFromBuffer(&localStack, btDbvt::DOUBLE_STACKSIZE, btDbvt::DOUBLE_STACKSIZE);
 239 #else
 240                 stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
 241 #endif
 242                 stkStack[0] = btDbvt::sStkNN(root0, root1);
 243                 do
 244                 {
 245                         btDbvt::sStkNN p = stkStack[--depth];
 246                         if (MyIntersect(p.a->volume, p.b->volume, xform, distanceThreshold))
 247                         {
 248                                 if (depth > treshold)
 249                                 {
 250                                         stkStack.resize(stkStack.size() * 2);
 251                                         treshold = stkStack.size() - 4;
 252                                 }
 253                                 if (p.a->isinternal())
 254                                 {
 255                                         if (p.b->isinternal())
 256                                         {
 257                                                 stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[0]);
 258                                                 stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[0]);
 259                                                 stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[1]);
 260                                                 stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[1]);
 261                                         }
 262                                         else
 263                                         {
 264                                                 stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b);
 265                                                 stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b);
 266                                         }
 267                                 }
 268                                 else
 269                                 {
 270                                         if (p.b->isinternal())
 271                                         {
 272                                                 stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[0]);
 273                                                 stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[1]);
 274                                         }
 275                                         else
 276                                         {
 277                                                 callback->Process(p.a, p.b);
 278                                         }
 279                                 }
 280                         }
 281                 } while (depth);
 282         }
 283 }
 284
 285 void btCompoundCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 286 {
 287         const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
 288         const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
 289
 290         btAssert(col0ObjWrap->getCollisionShape()->isCompound());
 291         btAssert(col1ObjWrap->getCollisionShape()->isCompound());
 292         const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
 293         const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
 294
 295         const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
 296         const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
 297         if (!tree0 || !tree1)
 298         {
 299                 return btCompoundCollisionAlgorithm::processCollision(body0Wrap, body1Wrap, dispatchInfo, resultOut);
 300         }
 301         ///btCompoundShape might have changed:
 302         ////make sure the internal child collision algorithm caches are still valid
 303         if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
 304         {
 305                 ///clear all
 306                 removeChildAlgorithms();
 307                 m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
 308                 m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
 309         }
 310
 311         ///we need to refresh all contact manifolds
 312         ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
 313         ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
 314         {
 315                 int i;
 316                 btManifoldArray manifoldArray;
 317 #ifdef USE_LOCAL_STACK
 318                 btPersistentManifold localManifolds[4];
 319                 manifoldArray.initializeFromBuffer(&localManifolds, 0, 4);
 320 #endif
 321                 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
 322                 for (i = 0; i < pairs.size(); i++)
 323                 {
 324                         if (pairs[i].m_userPointer)
 325                         {
 326                                 btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
 327                                 algo->getAllContactManifolds(manifoldArray);
 328                                 for (int m = 0; m < manifoldArray.size(); m++)
 329                                 {
 330                                         if (manifoldArray[m]->getNumContacts())
 331                                         {
 332                                                 resultOut->setPersistentManifold(manifoldArray[m]);
 333                                                 resultOut->refreshContactPoints();
 334                                                 resultOut->setPersistentManifold(0);
 335                                         }
 336                                 }
 337                                 manifoldArray.resize(0);
 338                         }
 339                 }
 340         }
 341
 342         btCompoundCompoundLeafCallback callback(col0ObjWrap, col1ObjWrap, this->m_dispatcher, dispatchInfo, resultOut, this->m_childCollisionAlgorithmCache, m_sharedManifold);
 343
 344         const btTransform xform = col0ObjWrap->getWorldTransform().inverse() * col1ObjWrap->getWorldTransform();
 345         MycollideTT(tree0->m_root, tree1->m_root, xform, &callback, resultOut->m_closestPointDistanceThreshold);
 346
 347         //printf("#compound-compound child/leaf overlap =%d                      \r",callback.m_numOverlapPairs);
 348
 349         //remove non-overlapping child pairs
 350
 351         {
 352                 btAssert(m_removePairs.size() == 0);
 353
 354                 //iterate over all children, perform an AABB check inside ProcessChildShape
 355                 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
 356
 357                 int i;
 358                 btManifoldArray manifoldArray;
 359
 360                 btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
 361
 362                 for (i = 0; i < pairs.size(); i++)
 363                 {
 364                         if (pairs[i].m_userPointer)
 365                         {
 366                                 btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
 367
 368                                 {
 369                                         const btCollisionShape* childShape0 = 0;
 370
 371                                         btTransform newChildWorldTrans0;
 372                                         childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
 373                                         const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
 374                                         newChildWorldTrans0 = col0ObjWrap->getWorldTransform() * childTrans0;
 375                                         childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0);
 376                                 }
 377                                 btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
 378                                 aabbMin0 -= thresholdVec;
 379                                 aabbMax0 += thresholdVec;
 380                                 {
 381                                         const btCollisionShape* childShape1 = 0;
 382                                         btTransform newChildWorldTrans1;
 383
 384                                         childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
 385                                         const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
 386                                         newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1;
 387                                         childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1);
 388                                 }
 389
 390                                 aabbMin1 -= thresholdVec;
 391                                 aabbMax1 += thresholdVec;
 392
 393                                 if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 394                                 {
 395                                         algo->~btCollisionAlgorithm();
 396                                         m_dispatcher->freeCollisionAlgorithm(algo);
 397                                         m_removePairs.push_back(btSimplePair(pairs[i].m_indexA, pairs[i].m_indexB));
 398                                 }
 399                         }
 400                 }
 401                 for (int i = 0; i < m_removePairs.size(); i++)
 402                 {
 403                         m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA, m_removePairs[i].m_indexB);
 404                 }
 405                 m_removePairs.clear();
 406         }
 407 }
 408
 409 btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 410 {
 411         btAssert(0);
 412         return 0.f;
 413 }