src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

   1
   2 /*
   3 Bullet Continuous Collision Detection and Physics Library
   4 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   5
   6 This software is provided 'as-is', without any express or implied warranty.
   7 In no event will the authors be held liable for any damages arising from the use of this software.
   8 Permission is granted to anyone to use this software for any purpose,
   9 including commercial applications, and to alter it and redistribute it freely,
  10 subject to the following restrictions:
  11
  12 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.
  13 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  14 3. This notice may not be removed or altered from any source distribution.
  15 */
  16
  17
  18 #include "LinearMath/btScalar.h"
  19 #include "btSimulationIslandManager.h"
  20 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
  21 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
  22 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  23 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
  24
  25 //#include <stdio.h>
  26 #include "LinearMath/btQuickprof.h"
  27
  28 btSimulationIslandManager::btSimulationIslandManager():
  29 m_splitIslands(true)
  30 {
  31 }
  32
  33 btSimulationIslandManager::~btSimulationIslandManager()
  34 {
  35 }
  36
  37
  38 void btSimulationIslandManager::initUnionFind(int n)
  39 {
  40                 m_unionFind.reset(n);
  41 }
  42
  43
  44 void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
  45 {
  46
  47         {
  48                 btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
  49                 const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
  50                 if (numOverlappingPairs)
  51                 {
  52                 btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
  53
  54                 for (int i=0;i<numOverlappingPairs;i++)
  55                 {
  56                         const btBroadphasePair& collisionPair = pairPtr[i];
  57                         btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
  58                         btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
  59
  60                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
  61                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
  62                         {
  63
  64                                 m_unionFind.unite((colObj0)->getIslandTag(),
  65                                         (colObj1)->getIslandTag());
  66                         }
  67                 }
  68                 }
  69         }
  70 }
  71
  72 #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
  73 void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
  74 {
  75
  76         // put the index into m_controllers into m_tag
  77         int index = 0;
  78         {
  79
  80                 int i;
  81                 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
  82                 {
  83                         btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
  84                         //Adding filtering here
  85                         if (!collisionObject->isStaticOrKinematicObject())
  86                         {
  87                                 collisionObject->setIslandTag(index++);
  88                         }
  89                         collisionObject->setCompanionId(-1);
  90                         collisionObject->setHitFraction(btScalar(1.));
  91                 }
  92         }
  93         // do the union find
  94
  95         initUnionFind( index );
  96
  97         findUnions(dispatcher,colWorld);
  98 }
  99
 100 void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
 101 {
 102         // put the islandId ('find' value) into m_tag
 103         {
 104                 int index = 0;
 105                 int i;
 106                 for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
 107                 {
 108                         btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
 109                         if (!collisionObject->isStaticOrKinematicObject())
 110                         {
 111                                 collisionObject->setIslandTag( m_unionFind.find(index) );
 112                                 //Set the correct object offset in Collision Object Array
 113                                 m_unionFind.getElement(index).m_sz = i;
 114                                 collisionObject->setCompanionId(-1);
 115                                 index++;
 116                         } else
 117                         {
 118                                 collisionObject->setIslandTag(-1);
 119                                 collisionObject->setCompanionId(-2);
 120                         }
 121                 }
 122         }
 123 }
 124
 125
 126 #else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
 127 void    btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
 128 {
 129
 130         initUnionFind( int (colWorld->getCollisionObjectArray().size()));
 131
 132         // put the index into m_controllers into m_tag
 133         {
 134
 135                 int index = 0;
 136                 int i;
 137                 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
 138                 {
 139                         btCollisionObject*      collisionObject= colWorld->getCollisionObjectArray()[i];
 140                         collisionObject->setIslandTag(index);
 141                         collisionObject->setCompanionId(-1);
 142                         collisionObject->setHitFraction(btScalar(1.));
 143                         index++;
 144
 145                 }
 146         }
 147         // do the union find
 148
 149         findUnions(dispatcher,colWorld);
 150 }
 151
 152 void    btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
 153 {
 154         // put the islandId ('find' value) into m_tag
 155         {
 156
 157
 158                 int index = 0;
 159                 int i;
 160                 for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
 161                 {
 162                         btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
 163                         if (!collisionObject->isStaticOrKinematicObject())
 164                         {
 165                                 collisionObject->setIslandTag( m_unionFind.find(index) );
 166                                 collisionObject->setCompanionId(-1);
 167                         } else
 168                         {
 169                                 collisionObject->setIslandTag(-1);
 170                                 collisionObject->setCompanionId(-2);
 171                         }
 172                         index++;
 173                 }
 174         }
 175 }
 176
 177 #endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
 178
 179 inline  int     getIslandId(const btPersistentManifold* lhs)
 180 {
 181         int islandId;
 182         const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
 183         const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
 184         islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
 185         return islandId;
 186
 187 }
 188
 189
 190
 191 /// function object that routes calls to operator<
 192 class btPersistentManifoldSortPredicate
 193 {
 194         public:
 195
 196                 SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const
 197                 {
 198                         return getIslandId(lhs) < getIslandId(rhs);
 199                 }
 200 };
 201
 202
 203 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
 204 {
 205
 206         BT_PROFILE("islandUnionFindAndQuickSort");
 207
 208         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 209
 210         m_islandmanifold.resize(0);
 211
 212         //we are going to sort the unionfind array, and store the element id in the size
 213         //afterwards, we clean unionfind, to make sure no-one uses it anymore
 214
 215         getUnionFind().sortIslands();
 216         int numElem = getUnionFind().getNumElements();
 217
 218         int endIslandIndex=1;
 219         int startIslandIndex;
 220
 221
 222         //update the sleeping state for bodies, if all are sleeping
 223         for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
 224         {
 225                 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
 226                 for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
 227                 {
 228                 }
 229
 230                 //int numSleeping = 0;
 231
 232                 bool allSleeping = true;
 233
 234                 int idx;
 235                 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
 236                 {
 237                         int i = getUnionFind().getElement(idx).m_sz;
 238
 239                         btCollisionObject* colObj0 = collisionObjects[i];
 240                         if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 241                         {
 242 //                              printf("error in island management\n");
 243                         }
 244
 245                         btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 246                         if (colObj0->getIslandTag() == islandId)
 247                         {
 248                                 if (colObj0->getActivationState()== ACTIVE_TAG)
 249                                 {
 250                                         allSleeping = false;
 251                                 }
 252                                 if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
 253                                 {
 254                                         allSleeping = false;
 255                                 }
 256                         }
 257                 }
 258
 259
 260                 if (allSleeping)
 261                 {
 262                         int idx;
 263                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
 264                         {
 265                                 int i = getUnionFind().getElement(idx).m_sz;
 266                                 btCollisionObject* colObj0 = collisionObjects[i];
 267                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 268                                 {
 269 //                                      printf("error in island management\n");
 270                                 }
 271
 272                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 273
 274                                 if (colObj0->getIslandTag() == islandId)
 275                                 {
 276                                         colObj0->setActivationState( ISLAND_SLEEPING );
 277                                 }
 278                         }
 279                 } else
 280                 {
 281
 282                         int idx;
 283                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
 284                         {
 285                                 int i = getUnionFind().getElement(idx).m_sz;
 286
 287                                 btCollisionObject* colObj0 = collisionObjects[i];
 288                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 289                                 {
 290 //                                      printf("error in island management\n");
 291                                 }
 292
 293                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 294
 295                                 if (colObj0->getIslandTag() == islandId)
 296                                 {
 297                                         if ( colObj0->getActivationState() == ISLAND_SLEEPING)
 298                                         {
 299                                                 colObj0->setActivationState( WANTS_DEACTIVATION);
 300                                                 colObj0->setDeactivationTime(0.f);
 301                                         }
 302                                 }
 303                         }
 304                 }
 305         }
 306
 307
 308         int i;
 309         int maxNumManifolds = dispatcher->getNumManifolds();
 310
 311 //#define SPLIT_ISLANDS 1
 312 //#ifdef SPLIT_ISLANDS
 313
 314
 315 //#endif //SPLIT_ISLANDS
 316
 317
 318         for (i=0;i<maxNumManifolds ;i++)
 319         {
 320                  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
 321
 322                  const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
 323                  const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
 324
 325                  ///@todo: check sleeping conditions!
 326                  if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
 327                         ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
 328                 {
 329
 330                         //kinematic objects don't merge islands, but wake up all connected objects
 331                         if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
 332                         {
 333                                 if (colObj0->hasContactResponse())
 334                                         colObj1->activate();
 335                         }
 336                         if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
 337                         {
 338                                 if (colObj1->hasContactResponse())
 339                                         colObj0->activate();
 340                         }
 341                         if(m_splitIslands)
 342                         {
 343                                 //filtering for response
 344                                 if (dispatcher->needsResponse(colObj0,colObj1))
 345                                         m_islandmanifold.push_back(manifold);
 346                         }
 347                 }
 348         }
 349 }
 350
 351
 352
 353 ///@todo: this is random access, it can be walked 'cache friendly'!
 354 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
 355 {
 356         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 357
 358         buildIslands(dispatcher,collisionWorld);
 359
 360         int endIslandIndex=1;
 361         int startIslandIndex;
 362         int numElem = getUnionFind().getNumElements();
 363
 364         BT_PROFILE("processIslands");
 365
 366         if(!m_splitIslands)
 367         {
 368                 btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
 369                 int maxNumManifolds = dispatcher->getNumManifolds();
 370                 callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
 371         }
 372         else
 373         {
 374                 // Sort manifolds, based on islands
 375                 // Sort the vector using predicate and std::sort
 376                 //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
 377
 378                 int numManifolds = int (m_islandmanifold.size());
 379
 380                 //tried a radix sort, but quicksort/heapsort seems still faster
 381                 //@todo rewrite island management
 382                 m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
 383                 //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
 384
 385                 //now process all active islands (sets of manifolds for now)
 386
 387                 int startManifoldIndex = 0;
 388                 int endManifoldIndex = 1;
 389
 390                 //int islandId;
 391
 392
 393
 394         //      printf("Start Islands\n");
 395
 396                 //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
 397                 for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
 398                 {
 399                         int islandId = getUnionFind().getElement(startIslandIndex).m_id;
 400
 401
 402                            bool islandSleeping = true;
 403
 404                                         for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
 405                                         {
 406                                                         int i = getUnionFind().getElement(endIslandIndex).m_sz;
 407                                                         btCollisionObject* colObj0 = collisionObjects[i];
 408                                                         m_islandBodies.push_back(colObj0);
 409                                                         if (colObj0->isActive())
 410                                                                         islandSleeping = false;
 411                                         }
 412
 413
 414                         //find the accompanying contact manifold for this islandId
 415                         int numIslandManifolds = 0;
 416                         btPersistentManifold** startManifold = 0;
 417
 418                         if (startManifoldIndex<numManifolds)
 419                         {
 420                                 int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
 421                                 if (curIslandId == islandId)
 422                                 {
 423                                         startManifold = &m_islandmanifold[startManifoldIndex];
 424
 425                                         for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
 426                                         {
 427
 428                                         }
 429                                         /// Process the actual simulation, only if not sleeping/deactivated
 430                                         numIslandManifolds = endManifoldIndex-startManifoldIndex;
 431                                 }
 432
 433                         }
 434
 435                         if (!islandSleeping)
 436                         {
 437                                 callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
 438         //                      printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
 439                         }
 440
 441                         if (numIslandManifolds)
 442                         {
 443                                 startManifoldIndex = endManifoldIndex;
 444                         }
 445
 446                         m_islandBodies.resize(0);
 447                 }
 448         } // else if(!splitIslands)
 449
 450 }