Bullet Collision Detection & Physics Library
btSimulationIslandManagerMt.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#include "LinearMath/btScalar.h"
24#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h" // for s_minimumContactManifoldsForBatching
25
26//#include <stdio.h>
28
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
37{
38 return calcBatchCost(island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size());
39}
40
42{
47}
48
50{
51 for (int i = 0; i < m_allocatedIslands.size(); ++i)
52 {
53 delete m_allocatedIslands[i];
54 }
58}
59
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
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
78{
79public:
81 {
82 int lCost = calcBatchCost(lhs);
83 int rCost = calcBatchCost(rhs);
84 return lCost > rCost;
85 }
86};
87
89{
90public:
92 {
93 return lhs->bodyArray.capacity() > rhs->bodyArray.capacity();
94 }
95};
96
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 {
108 }
109 // append constraints
110 for (int i = 0; i < other.constraintArray.size(); ++i)
111 {
113 }
114}
115
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
129{
130 // reset island pools
133 for (int i = 0; i < m_lookupIslandFromId.size(); ++i)
134 {
136 }
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 {
145 int cap = island->bodyArray.capacity();
146 if (cap > lastCapacity)
147 {
148 isSorted = false;
149 break;
150 }
152 }
153 if (!isSorted)
154 {
156 }
157
159 // mark all islands free (but avoid deallocation)
160 for (int i = 0; i < m_allocatedIslands.size(); ++i)
161 {
163 island->bodyArray.resize(0);
164 island->manifoldArray.resize(0);
165 island->constraintArray.resize(0);
166 island->id = -1;
167 island->isSleeping = true;
169 }
170}
171
173{
174 btAssert(id >= 0);
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 {
185 break;
186 }
187 }
189 }
190 return island;
191}
192
194{
195 Island* island = NULL;
196 int allocSize = numBodies;
197 if (numBodies < m_batchIslandMinBodyCount)
198 {
199 if (m_batchIsland)
200 {
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
208 }
209 return island;
210 }
211 else
212 {
213 // need to allocate a batch island
214 allocSize = m_batchIslandMinBodyCount * 2;
215 }
216 }
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 {
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);
254 }
256 if (numBodies < m_batchIslandMinBodyCount)
257 {
259 }
261 return island;
262}
263
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
275
276 int endIslandIndex = 1;
278
279 //update the sleeping state for bodies, if all are sleeping
281 {
284 {
285 }
286
287 //int numSleeping = 0;
288
289 bool allSleeping = true;
290
291 int idx;
293 {
294 int i = getUnionFind().getElement(idx).m_sz;
295
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;
318 {
319 int i = getUnionFind().getElement(idx).m_sz;
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;
338 {
339 int i = getUnionFind().getElement(idx).m_sz;
340
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
363{
364 btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
365 int endIslandIndex = 1;
368
369 // create explicit islands and add bodies to each
371 {
373
374 // find end index
376 {
377 }
378 // check if island is sleeping
379 bool islandSleeping = true;
381 {
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
398 {
401 island->bodyArray.push_back(colObj);
402 }
403 }
404 }
405}
406
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
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
438 // if island not sleeping,
440 {
441 island->manifoldArray.push_back(manifold);
442 }
443 }
444 }
445 }
446}
447
449{
450 // walk constraints
451 for (int i = 0; i < constraints.size(); i++)
452 {
453 // scatter constraints into various islands
455 if (constraint->isEnabled())
456 {
458 // if island is not sleeping,
460 {
461 island->constraintArray.push_back(constraint);
462 }
463 }
464 }
465}
466
468{
469 // sort islands in order of decreasing batch size
471
472 // merge small islands to satisfy minimum batch size
473 // find first small batch island
475 for (int i = 0; i < m_activeIslands.size(); ++i)
476 {
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
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 {
498 numBodies += src->bodyArray.size();
499 numManifolds += src->manifoldArray.size();
500 numConstraints += src->constraintArray.size();
501 int batchCost = calcBatchCost(numBodies, numManifolds, numConstraints);
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
523 lastIndex = firstIndex - 1;
525 }
526}
527
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(),
537 island.constraintArray.size(),
538 *solverParams.m_solverInfo,
539 solverParams.m_debugDrawer,
540 solverParams.m_dispatcher);
541}
542
544{
545 BT_PROFILE("serialIslandDispatch");
546 // serial dispatch
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
556{
559
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 {
571 btSimulationIslandManagerMt::solveIsland(solver, *island, m_solverParams);
572 }
573 }
574};
575
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
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 {
608 {
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)
617 ++iBegin;
618 }
619 }
620 // parallel dispatch to sequential solvers for rest
622}
623
629{
630 BT_PROFILE("buildAndProcessIslands");
631 btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
632
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 {
643
644 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
645 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
646
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 }
665 btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
666 solver->solveGroup(&collisionObjects[0],
667 collisionObjects.size(),
668 manifolds,
671 constraints.size(),
672 *solverParams.m_solverInfo,
673 solverParams.m_debugDrawer,
674 solverParams.m_dispatcher);
675 }
676 else
677 {
679
680 //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
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,
690 {
691 mergeIslands();
692 }
693 // dispatch islands to solver
695 }
696}
static void getElement(int arrayLen, const char *cur, const char *old, char *oldPtr, char *curData)
Definition bFile.cpp:817
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
#define ISLAND_SLEEPING
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition btDbvt.cpp:52
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
#define BT_PROFILE(name)
#define SIMD_FORCE_INLINE
Definition btScalar.h:98
#define btAssert(x)
Definition btScalar.h:153
int btGetConstraintIslandId1(const btTypedConstraint *lhs)
bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island &island, const btCollisionObject *obj)
int getIslandId(const btPersistentManifold *lhs)
int calcBatchCost(int bodies, int manifolds, int constraints)
int getIslandId(const btPersistentManifold *lhs)
void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody &body)
#define BT_OVERRIDE
Definition btThreads.h:26
function object that routes calls to operator<
bool operator()(const btSimulationIslandManagerMt::Island *lhs, const btSimulationIslandManagerMt::Island *rhs) const
bool operator()(const btSimulationIslandManagerMt::Island *lhs, const btSimulationIslandManagerMt::Island *rhs) const
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
void push_back(const T &_Val)
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
btCollisionObject can be used to manage collision detection objects.
void activate(bool forceActivation=false) const
CollisionWorld is interface and container for the collision detection.
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, class btIDebugDraw *debugDrawer, btDispatcher *dispatcher)=0
solve a group of constraints
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
btAlignedObjectArray< Island * > m_allocatedIslands
virtual void addBodiesToIslands(btCollisionWorld *collisionWorld)
virtual void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, btAlignedObjectArray< btTypedConstraint * > &constraints, const SolverParams &solverParams)
virtual Island * allocateIsland(int id, int numBodies)
btAlignedObjectArray< Island * > m_freeIslands
btAlignedObjectArray< Island * > m_lookupIslandFromId
virtual void addManifoldsToIslands(btDispatcher *dispatcher)
virtual void addConstraintsToIslands(btAlignedObjectArray< btTypedConstraint * > &constraints)
btAlignedObjectArray< Island * > m_activeIslands
static void parallelIslandDispatch(btAlignedObjectArray< Island * > *islandsPtr, const SolverParams &solverParams)
static void solveIsland(btConstraintSolver *solver, Island &island, const SolverParams &solverParams)
static void serialIslandDispatch(btAlignedObjectArray< Island * > *islandsPtr, const SolverParams &solverParams)
virtual void buildIslands(btDispatcher *dispatcher, btCollisionWorld *colWorld)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
int getNumElements() const
Definition btUnionFind.h:50
btElement & getElement(int index)
Definition btUnionFind.h:59
void sortIslands()
this is a special operation, destroying the content of btUnionFind.
UpdateIslandDispatcher(btAlignedObjectArray< btSimulationIslandManagerMt::Island * > &islandsPtr, const btSimulationIslandManagerMt::SolverParams &solverParams)
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btAlignedObjectArray< btSimulationIslandManagerMt::Island * > & m_islandsPtr
const btSimulationIslandManagerMt::SolverParams & m_solverParams
btAlignedObjectArray< btTypedConstraint * > constraintArray
btAlignedObjectArray< btCollisionObject * > bodyArray
btAlignedObjectArray< btPersistentManifold * > manifoldArray