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

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