Bullet Collision Detection & Physics Library: btSimulationIslandManagerMt.cpp Source File

Bullet Collision Detection & Physics Library

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