dali-physics/third-party/bullet3/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://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 #include "LinearMath/btScalar.h"
  17 #include "LinearMath/btThreads.h"
  18 #include "btSimulationIslandManagerMt.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 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
  24 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h"  // for s_minimumContactManifoldsForBatching
  25
  26 //#include <stdio.h>
  27 #include "LinearMath/btQuickprof.h"
  28
  29 SIMD_FORCE_INLINE int calcBatchCost(int bodies, int manifolds, int constraints)
  30 {
  31         // rough estimate of the cost of a batch, used for merging
  32         int batchCost = bodies + 8 * manifolds + 4 * constraints;
  33         return batchCost;
  34 }
  35
  36 SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island)
  37 {
  38         return calcBatchCost(island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size());
  39 }
  40
  41 btSimulationIslandManagerMt::btSimulationIslandManagerMt()
  42 {
  43         m_minimumSolverBatchSize = calcBatchCost(0, 128, 0);
  44         m_batchIslandMinBodyCount = 32;
  45         m_islandDispatch = parallelIslandDispatch;
  46         m_batchIsland = NULL;
  47 }
  48
  49 btSimulationIslandManagerMt::~btSimulationIslandManagerMt()
  50 {
  51         for (int i = 0; i < m_allocatedIslands.size(); ++i)
  52         {
  53                 delete m_allocatedIslands[i];
  54         }
  55         m_allocatedIslands.resize(0);
  56         m_activeIslands.resize(0);
  57         m_freeIslands.resize(0);
  58 }
  59
  60 inline int getIslandId(const btPersistentManifold* lhs)
  61 {
  62         const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
  63         const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
  64         int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
  65         return islandId;
  66 }
  67
  68 SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs)
  69 {
  70         const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
  71         const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
  72         int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
  73         return islandId;
  74 }
  75
  76 /// function object that routes calls to operator<
  77 class IslandBatchSizeSortPredicate
  78 {
  79 public:
  80         bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const
  81         {
  82                 int lCost = calcBatchCost(lhs);
  83                 int rCost = calcBatchCost(rhs);
  84                 return lCost > rCost;
  85         }
  86 };
  87
  88 class IslandBodyCapacitySortPredicate
  89 {
  90 public:
  91         bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const
  92         {
  93                 return lhs->bodyArray.capacity() > rhs->bodyArray.capacity();
  94         }
  95 };
  96
  97 void btSimulationIslandManagerMt::Island::append(const Island& other)
  98 {
  99         // append bodies
 100         for (int i = 0; i < other.bodyArray.size(); ++i)
 101         {
 102                 bodyArray.push_back(other.bodyArray[i]);
 103         }
 104         // append manifolds
 105         for (int i = 0; i < other.manifoldArray.size(); ++i)
 106         {
 107                 manifoldArray.push_back(other.manifoldArray[i]);
 108         }
 109         // append constraints
 110         for (int i = 0; i < other.constraintArray.size(); ++i)
 111         {
 112                 constraintArray.push_back(other.constraintArray[i]);
 113         }
 114 }
 115
 116 bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj)
 117 {
 118         for (int i = 0; i < island.bodyArray.size(); ++i)
 119         {
 120                 if (island.bodyArray[i] == obj)
 121                 {
 122                         return true;
 123                 }
 124         }
 125         return false;
 126 }
 127
 128 void btSimulationIslandManagerMt::initIslandPools()
 129 {
 130         // reset island pools
 131         int numElem = getUnionFind().getNumElements();
 132         m_lookupIslandFromId.resize(numElem);
 133         for (int i = 0; i < m_lookupIslandFromId.size(); ++i)
 134         {
 135                 m_lookupIslandFromId[i] = NULL;
 136         }
 137         m_activeIslands.resize(0);
 138         m_freeIslands.resize(0);
 139         // check whether allocated islands are sorted by body capacity (largest to smallest)
 140         int lastCapacity = 0;
 141         bool isSorted = true;
 142         for (int i = 0; i < m_allocatedIslands.size(); ++i)
 143         {
 144                 Island* island = m_allocatedIslands[i];
 145                 int cap = island->bodyArray.capacity();
 146                 if (cap > lastCapacity)
 147                 {
 148                         isSorted = false;
 149                         break;
 150                 }
 151                 lastCapacity = cap;
 152         }
 153         if (!isSorted)
 154         {
 155                 m_allocatedIslands.quickSort(IslandBodyCapacitySortPredicate());
 156         }
 157
 158         m_batchIsland = NULL;
 159         // mark all islands free (but avoid deallocation)
 160         for (int i = 0; i < m_allocatedIslands.size(); ++i)
 161         {
 162                 Island* island = m_allocatedIslands[i];
 163                 island->bodyArray.resize(0);
 164                 island->manifoldArray.resize(0);
 165                 island->constraintArray.resize(0);
 166                 island->id = -1;
 167                 island->isSleeping = true;
 168                 m_freeIslands.push_back(island);
 169         }
 170 }
 171
 172 btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
 173 {
 174         btAssert(id >= 0);
 175         btAssert(id < m_lookupIslandFromId.size());
 176         Island* island = m_lookupIslandFromId[id];
 177         if (island == NULL)
 178         {
 179                 // search for existing island
 180                 for (int i = 0; i < m_activeIslands.size(); ++i)
 181                 {
 182                         if (m_activeIslands[i]->id == id)
 183                         {
 184                                 island = m_activeIslands[i];
 185                                 break;
 186                         }
 187                 }
 188                 m_lookupIslandFromId[id] = island;
 189         }
 190         return island;
 191 }
 192
 193 btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland(int id, int numBodies)
 194 {
 195         Island* island = NULL;
 196         int allocSize = numBodies;
 197         if (numBodies < m_batchIslandMinBodyCount)
 198         {
 199                 if (m_batchIsland)
 200                 {
 201                         island = m_batchIsland;
 202                         m_lookupIslandFromId[id] = island;
 203                         // if we've made a large enough batch,
 204                         if (island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount)
 205                         {
 206                                 // next time start a new batch
 207                                 m_batchIsland = NULL;
 208                         }
 209                         return island;
 210                 }
 211                 else
 212                 {
 213                         // need to allocate a batch island
 214                         allocSize = m_batchIslandMinBodyCount * 2;
 215                 }
 216         }
 217         btAlignedObjectArray<Island*>& freeIslands = m_freeIslands;
 218
 219         // search for free island
 220         if (freeIslands.size() > 0)
 221         {
 222                 // try to reuse a previously allocated island
 223                 int iFound = freeIslands.size();
 224                 // linear search for smallest island that can hold our bodies
 225                 for (int i = freeIslands.size() - 1; i >= 0; --i)
 226                 {
 227                         if (freeIslands[i]->bodyArray.capacity() >= allocSize)
 228                         {
 229                                 iFound = i;
 230                                 island = freeIslands[i];
 231                                 island->id = id;
 232                                 break;
 233                         }
 234                 }
 235                 // if found, shrink array while maintaining ordering
 236                 if (island)
 237                 {
 238                         int iDest = iFound;
 239                         int iSrc = iDest + 1;
 240                         while (iSrc < freeIslands.size())
 241                         {
 242                                 freeIslands[iDest++] = freeIslands[iSrc++];
 243                         }
 244                         freeIslands.pop_back();
 245                 }
 246         }
 247         if (island == NULL)
 248         {
 249                 // no free island found, allocate
 250                 island = new Island();  // TODO: change this to use the pool allocator
 251                 island->id = id;
 252                 island->bodyArray.reserve(allocSize);
 253                 m_allocatedIslands.push_back(island);
 254         }
 255         m_lookupIslandFromId[id] = island;
 256         if (numBodies < m_batchIslandMinBodyCount)
 257         {
 258                 m_batchIsland = island;
 259         }
 260         m_activeIslands.push_back(island);
 261         return island;
 262 }
 263
 264 void btSimulationIslandManagerMt::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld)
 265 {
 266         BT_PROFILE("buildIslands");
 267
 268         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 269
 270         //we are going to sort the unionfind array, and store the element id in the size
 271         //afterwards, we clean unionfind, to make sure no-one uses it anymore
 272
 273         getUnionFind().sortIslands();
 274         int numElem = getUnionFind().getNumElements();
 275
 276         int endIslandIndex = 1;
 277         int startIslandIndex;
 278
 279         //update the sleeping state for bodies, if all are sleeping
 280         for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
 281         {
 282                 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
 283                 for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
 284                 {
 285                 }
 286
 287                 //int numSleeping = 0;
 288
 289                 bool allSleeping = true;
 290
 291                 int idx;
 292                 for (idx = startIslandIndex; idx < endIslandIndex; idx++)
 293                 {
 294                         int i = getUnionFind().getElement(idx).m_sz;
 295
 296                         btCollisionObject* colObj0 = collisionObjects[i];
 297                         if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 298                         {
 299                                 //                              printf("error in island management\n");
 300                         }
 301
 302                         btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 303                         if (colObj0->getIslandTag() == islandId)
 304                         {
 305                                 if (colObj0->getActivationState() == ACTIVE_TAG ||
 306                                         colObj0->getActivationState() == DISABLE_DEACTIVATION)
 307                                 {
 308                                         allSleeping = false;
 309                                         break;
 310                                 }
 311                         }
 312                 }
 313
 314                 if (allSleeping)
 315                 {
 316                         int idx;
 317                         for (idx = startIslandIndex; idx < endIslandIndex; idx++)
 318                         {
 319                                 int i = getUnionFind().getElement(idx).m_sz;
 320                                 btCollisionObject* colObj0 = collisionObjects[i];
 321                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 322                                 {
 323                                         //                                      printf("error in island management\n");
 324                                 }
 325
 326                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 327
 328                                 if (colObj0->getIslandTag() == islandId)
 329                                 {
 330                                         colObj0->setActivationState(ISLAND_SLEEPING);
 331                                 }
 332                         }
 333                 }
 334                 else
 335                 {
 336                         int idx;
 337                         for (idx = startIslandIndex; idx < endIslandIndex; idx++)
 338                         {
 339                                 int i = getUnionFind().getElement(idx).m_sz;
 340
 341                                 btCollisionObject* colObj0 = collisionObjects[i];
 342                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 343                                 {
 344                                         //                                      printf("error in island management\n");
 345                                 }
 346
 347                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 348
 349                                 if (colObj0->getIslandTag() == islandId)
 350                                 {
 351                                         if (colObj0->getActivationState() == ISLAND_SLEEPING)
 352                                         {
 353                                                 colObj0->setActivationState(WANTS_DEACTIVATION);
 354                                                 colObj0->setDeactivationTime(0.f);
 355                                         }
 356                                 }
 357                         }
 358                 }
 359         }
 360 }
 361
 362 void btSimulationIslandManagerMt::addBodiesToIslands(btCollisionWorld* collisionWorld)
 363 {
 364         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 365         int endIslandIndex = 1;
 366         int startIslandIndex;
 367         int numElem = getUnionFind().getNumElements();
 368
 369         // create explicit islands and add bodies to each
 370         for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
 371         {
 372                 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
 373
 374                 // find end index
 375                 for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
 376                 {
 377                 }
 378                 // check if island is sleeping
 379                 bool islandSleeping = true;
 380                 for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++)
 381                 {
 382                         int i = getUnionFind().getElement(iElem).m_sz;
 383                         btCollisionObject* colObj = collisionObjects[i];
 384                         if (colObj->isActive())
 385                         {
 386                                 islandSleeping = false;
 387                         }
 388                 }
 389                 if (!islandSleeping)
 390                 {
 391                         // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures
 392                         int numBodies = endIslandIndex - startIslandIndex;
 393                         Island* island = allocateIsland(islandId, numBodies);
 394                         island->isSleeping = false;
 395
 396                         // add bodies to island
 397                         for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++)
 398                         {
 399                                 int i = getUnionFind().getElement(iElem).m_sz;
 400                                 btCollisionObject* colObj = collisionObjects[i];
 401                                 island->bodyArray.push_back(colObj);
 402                         }
 403                 }
 404         }
 405 }
 406
 407 void btSimulationIslandManagerMt::addManifoldsToIslands(btDispatcher* dispatcher)
 408 {
 409         // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island
 410         int maxNumManifolds = dispatcher->getNumManifolds();
 411         for (int i = 0; i < maxNumManifolds; i++)
 412         {
 413                 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
 414
 415                 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
 416                 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
 417
 418                 ///@todo: check sleeping conditions!
 419                 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
 420                         ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
 421                 {
 422                         //kinematic objects don't merge islands, but wake up all connected objects
 423                         if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
 424                         {
 425                                 if (colObj0->hasContactResponse())
 426                                         colObj1->activate();
 427                         }
 428                         if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
 429                         {
 430                                 if (colObj1->hasContactResponse())
 431                                         colObj0->activate();
 432                         }
 433                         //filtering for response
 434                         if (dispatcher->needsResponse(colObj0, colObj1))
 435                         {
 436                                 // scatter manifolds into various islands
 437                                 int islandId = getIslandId(manifold);
 438                                 // if island not sleeping,
 439                                 if (Island* island = getIsland(islandId))
 440                                 {
 441                                         island->manifoldArray.push_back(manifold);
 442                                 }
 443                         }
 444                 }
 445         }
 446 }
 447
 448 void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints)
 449 {
 450         // walk constraints
 451         for (int i = 0; i < constraints.size(); i++)
 452         {
 453                 // scatter constraints into various islands
 454                 btTypedConstraint* constraint = constraints[i];
 455                 if (constraint->isEnabled())
 456                 {
 457                         int islandId = btGetConstraintIslandId1(constraint);
 458                         // if island is not sleeping,
 459                         if (Island* island = getIsland(islandId))
 460                         {
 461                                 island->constraintArray.push_back(constraint);
 462                         }
 463                 }
 464         }
 465 }
 466
 467 void btSimulationIslandManagerMt::mergeIslands()
 468 {
 469         // sort islands in order of decreasing batch size
 470         m_activeIslands.quickSort(IslandBatchSizeSortPredicate());
 471
 472         // merge small islands to satisfy minimum batch size
 473         // find first small batch island
 474         int destIslandIndex = m_activeIslands.size();
 475         for (int i = 0; i < m_activeIslands.size(); ++i)
 476         {
 477                 Island* island = m_activeIslands[i];
 478                 int batchSize = calcBatchCost(island);
 479                 if (batchSize < m_minimumSolverBatchSize)
 480                 {
 481                         destIslandIndex = i;
 482                         break;
 483                 }
 484         }
 485         int lastIndex = m_activeIslands.size() - 1;
 486         while (destIslandIndex < lastIndex)
 487         {
 488                 // merge islands from the back of the list
 489                 Island* island = m_activeIslands[destIslandIndex];
 490                 int numBodies = island->bodyArray.size();
 491                 int numManifolds = island->manifoldArray.size();
 492                 int numConstraints = island->constraintArray.size();
 493                 int firstIndex = lastIndex;
 494                 // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have
 495                 while (true)
 496                 {
 497                         Island* src = m_activeIslands[firstIndex];
 498                         numBodies += src->bodyArray.size();
 499                         numManifolds += src->manifoldArray.size();
 500                         numConstraints += src->constraintArray.size();
 501                         int batchCost = calcBatchCost(numBodies, numManifolds, numConstraints);
 502                         if (batchCost >= m_minimumSolverBatchSize)
 503                         {
 504                                 break;
 505                         }
 506                         if (firstIndex - 1 == destIslandIndex)
 507                         {
 508                                 break;
 509                         }
 510                         firstIndex--;
 511                 }
 512                 // reserve space for these pointers to minimize reallocation
 513                 island->bodyArray.reserve(numBodies);
 514                 island->manifoldArray.reserve(numManifolds);
 515                 island->constraintArray.reserve(numConstraints);
 516                 // merge islands
 517                 for (int i = firstIndex; i <= lastIndex; ++i)
 518                 {
 519                         island->append(*m_activeIslands[i]);
 520                 }
 521                 // shrink array to exclude the islands that were merged from
 522                 m_activeIslands.resize(firstIndex);
 523                 lastIndex = firstIndex - 1;
 524                 destIslandIndex++;
 525         }
 526 }
 527
 528 void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams)
 529 {
 530         btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[0] : NULL;
 531         btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[0] : NULL;
 532         solver->solveGroup(&island.bodyArray[0],
 533                                            island.bodyArray.size(),
 534                                            manifolds,
 535                                            island.manifoldArray.size(),
 536                                            constraintsPtr,
 537                                            island.constraintArray.size(),
 538                                            *solverParams.m_solverInfo,
 539                                            solverParams.m_debugDrawer,
 540                                            solverParams.m_dispatcher);
 541 }
 542
 543 void btSimulationIslandManagerMt::serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams)
 544 {
 545         BT_PROFILE("serialIslandDispatch");
 546         // serial dispatch
 547         btAlignedObjectArray<Island*>& islands = *islandsPtr;
 548         btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
 549         for (int i = 0; i < islands.size(); ++i)
 550         {
 551                 solveIsland(solver, *islands[i], solverParams);
 552         }
 553 }
 554
 555 struct UpdateIslandDispatcher : public btIParallelForBody
 556 {
 557         btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr;
 558         const btSimulationIslandManagerMt::SolverParams& m_solverParams;
 559
 560         UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
 561                 : m_islandsPtr(islandsPtr), m_solverParams(solverParams)
 562         {
 563         }
 564
 565         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
 566         {
 567                 btConstraintSolver* solver = m_solverParams.m_solverPool;
 568                 for (int i = iBegin; i < iEnd; ++i)
 569                 {
 570                         btSimulationIslandManagerMt::Island* island = m_islandsPtr[i];
 571                         btSimulationIslandManagerMt::solveIsland(solver, *island, m_solverParams);
 572                 }
 573         }
 574 };
 575
 576 void btSimulationIslandManagerMt::parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams)
 577 {
 578         BT_PROFILE("parallelIslandDispatch");
 579         //
 580         // if there are islands with many contacts, it may be faster to submit these
 581         // large islands *serially* to a single parallel constraint solver, and then later
 582         // submit the remaining smaller islands in parallel to multiple sequential solvers.
 583         //
 584         // Some task schedulers do not deal well with nested parallelFor loops. One implementation
 585         // of OpenMP was actually slower than doing everything single-threaded. Intel TBB
 586         // on the other hand, seems to do a pretty respectable job with it.
 587         //
 588         // When solving islands in parallel, the worst case performance happens when there
 589         // is one very large island and then perhaps a smattering of very small
 590         // islands -- one worker thread takes the large island and the remaining workers
 591         // tear through the smaller islands and then sit idle waiting for the first worker
 592         // to finish. Solving islands in parallel works best when there are numerous small
 593         // islands, roughly equal in size.
 594         //
 595         // By contrast, the other approach -- the parallel constraint solver -- is only
 596         // able to deliver a worthwhile speedup when the island is large. For smaller islands,
 597         // it is difficult to extract a useful amount of parallelism -- the overhead of grouping
 598         // the constraints into batches and sending the batches to worker threads can nullify
 599         // any gains from parallelism.
 600         //
 601
 602         UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams);
 603         // We take advantage of the fact the islands are sorted in order of decreasing size
 604         int iBegin = 0;
 605         if (solverParams.m_solverMt)
 606         {
 607                 while (iBegin < islandsPtr->size())
 608                 {
 609                         btSimulationIslandManagerMt::Island* island = (*islandsPtr)[iBegin];
 610                         if (island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching)
 611                         {
 612                                 // OK to submit the rest of the array in parallel
 613                                 break;
 614                         }
 615                         // serial dispatch to parallel solver for large islands (if any)
 616                         solveIsland(solverParams.m_solverMt, *island, solverParams);
 617                         ++iBegin;
 618                 }
 619         }
 620         // parallel dispatch to sequential solvers for rest
 621         btParallelFor(iBegin, islandsPtr->size(), 1, dispatcher);
 622 }
 623
 624 ///@todo: this is random access, it can be walked 'cache friendly'!
 625 void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher,
 626                                                                                                                  btCollisionWorld* collisionWorld,
 627                                                                                                                  btAlignedObjectArray<btTypedConstraint*>& constraints,
 628                                                                                                                  const SolverParams& solverParams)
 629 {
 630         BT_PROFILE("buildAndProcessIslands");
 631         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 632
 633         buildIslands(dispatcher, collisionWorld);
 634
 635         if (!getSplitIslands())
 636         {
 637                 btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
 638                 int maxNumManifolds = dispatcher->getNumManifolds();
 639
 640                 for (int i = 0; i < maxNumManifolds; i++)
 641                 {
 642                         btPersistentManifold* manifold = manifolds[i];
 643
 644                         const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
 645                         const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
 646
 647                         ///@todo: check sleeping conditions!
 648                         if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
 649                                 ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
 650                         {
 651                                 //kinematic objects don't merge islands, but wake up all connected objects
 652                                 if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
 653                                 {
 654                                         if (colObj0->hasContactResponse())
 655                                                 colObj1->activate();
 656                                 }
 657                                 if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
 658                                 {
 659                                         if (colObj1->hasContactResponse())
 660                                                 colObj0->activate();
 661                                 }
 662                         }
 663                 }
 664                 btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[0] : NULL;
 665                 btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
 666                 solver->solveGroup(&collisionObjects[0],
 667                                                    collisionObjects.size(),
 668                                                    manifolds,
 669                                                    maxNumManifolds,
 670                                                    constraintsPtr,
 671                                                    constraints.size(),
 672                                                    *solverParams.m_solverInfo,
 673                                                    solverParams.m_debugDrawer,
 674                                                    solverParams.m_dispatcher);
 675         }
 676         else
 677         {
 678                 initIslandPools();
 679
 680                 //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
 681                 addBodiesToIslands(collisionWorld);
 682                 addManifoldsToIslands(dispatcher);
 683                 addConstraintsToIslands(constraints);
 684
 685                 // m_activeIslands array should now contain all non-sleeping Islands, and each Island should
 686                 // have all the necessary bodies, manifolds and constraints.
 687
 688                 // if we want to merge islands with small batch counts,
 689                 if (m_minimumSolverBatchSize > 1)
 690                 {
 691                         mergeIslands();
 692                 }
 693                 // dispatch islands to solver
 694                 m_islandDispatch(&m_activeIslands, solverParams);
 695         }
 696 }