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 
17 #include "LinearMath/btScalar.h"
24 
25 //#include <stdio.h>
26 #include "LinearMath/btQuickprof.h"
27 
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 
38 {
39  return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() );
40 }
41 
42 
44 {
48  m_batchIsland = NULL;
49 }
50 
51 
53 {
54  for ( int i = 0; i < m_allocatedIslands.size(); ++i )
55  {
56  delete m_allocatedIslands[ i ];
57  }
60  m_freeIslands.resize( 0 );
61 }
62 
63 
64 inline int getIslandId(const btPersistentManifold* lhs)
65 {
66  const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
67  const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
68  int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
69  return islandId;
70 }
71 
72 
74 {
75  const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
76  const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
77  int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
78  return islandId;
79 }
80 
83 {
84 public:
85  bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const
86  {
87  int lCost = calcBatchCost( lhs );
88  int rCost = calcBatchCost( rhs );
89  return lCost > rCost;
90  }
91 };
92 
93 
95 {
96 public:
97  bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const
98  {
99  return lhs->bodyArray.capacity() > rhs->bodyArray.capacity();
100  }
101 };
102 
103 
105 {
106  // append bodies
107  for ( int i = 0; i < other.bodyArray.size(); ++i )
108  {
109  bodyArray.push_back( other.bodyArray[ i ] );
110  }
111  // append manifolds
112  for ( int i = 0; i < other.manifoldArray.size(); ++i )
113  {
114  manifoldArray.push_back( other.manifoldArray[ i ] );
115  }
116  // append constraints
117  for ( int i = 0; i < other.constraintArray.size(); ++i )
118  {
119  constraintArray.push_back( other.constraintArray[ i ] );
120  }
121 }
122 
123 
125 {
126  for ( int i = 0; i < island.bodyArray.size(); ++i )
127  {
128  if ( island.bodyArray[ i ] == obj )
129  {
130  return true;
131  }
132  }
133  return false;
134 }
135 
136 
138 {
139  // reset island pools
140  int numElem = getUnionFind().getNumElements();
141  m_lookupIslandFromId.resize( numElem );
142  for ( int i = 0; i < m_lookupIslandFromId.size(); ++i )
143  {
144  m_lookupIslandFromId[ i ] = NULL;
145  }
146  m_activeIslands.resize( 0 );
147  m_freeIslands.resize( 0 );
148  // check whether allocated islands are sorted by body capacity (largest to smallest)
149  int lastCapacity = 0;
150  bool isSorted = true;
151  for ( int i = 0; i < m_allocatedIslands.size(); ++i )
152  {
153  Island* island = m_allocatedIslands[ i ];
154  int cap = island->bodyArray.capacity();
155  if ( cap > lastCapacity )
156  {
157  isSorted = false;
158  break;
159  }
160  lastCapacity = cap;
161  }
162  if ( !isSorted )
163  {
165  }
166 
167  m_batchIsland = NULL;
168  // mark all islands free (but avoid deallocation)
169  for ( int i = 0; i < m_allocatedIslands.size(); ++i )
170  {
171  Island* island = m_allocatedIslands[ i ];
172  island->bodyArray.resize( 0 );
173  island->manifoldArray.resize( 0 );
174  island->constraintArray.resize( 0 );
175  island->id = -1;
176  island->isSleeping = true;
177  m_freeIslands.push_back( island );
178  }
179 }
180 
181 
183 {
184  Island* island = m_lookupIslandFromId[ id ];
185  if ( island == NULL )
186  {
187  // search for existing island
188  for ( int i = 0; i < m_activeIslands.size(); ++i )
189  {
190  if ( m_activeIslands[ i ]->id == id )
191  {
192  island = m_activeIslands[ i ];
193  break;
194  }
195  }
196  m_lookupIslandFromId[ id ] = island;
197  }
198  return island;
199 }
200 
201 
203 {
204  Island* island = NULL;
205  int allocSize = numBodies;
206  if ( numBodies < m_batchIslandMinBodyCount )
207  {
208  if ( m_batchIsland )
209  {
210  island = m_batchIsland;
211  m_lookupIslandFromId[ id ] = island;
212  // if we've made a large enough batch,
213  if ( island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount )
214  {
215  // next time start a new batch
216  m_batchIsland = NULL;
217  }
218  return island;
219  }
220  else
221  {
222  // need to allocate a batch island
223  allocSize = m_batchIslandMinBodyCount * 2;
224  }
225  }
227 
228  // search for free island
229  if ( freeIslands.size() > 0 )
230  {
231  // try to reuse a previously allocated island
232  int iFound = freeIslands.size();
233  // linear search for smallest island that can hold our bodies
234  for ( int i = freeIslands.size() - 1; i >= 0; --i )
235  {
236  if ( freeIslands[ i ]->bodyArray.capacity() >= allocSize )
237  {
238  iFound = i;
239  island = freeIslands[ i ];
240  island->id = id;
241  break;
242  }
243  }
244  // if found, shrink array while maintaining ordering
245  if ( island )
246  {
247  int iDest = iFound;
248  int iSrc = iDest + 1;
249  while ( iSrc < freeIslands.size() )
250  {
251  freeIslands[ iDest++ ] = freeIslands[ iSrc++ ];
252  }
253  freeIslands.pop_back();
254  }
255  }
256  if ( island == NULL )
257  {
258  // no free island found, allocate
259  island = new Island(); // TODO: change this to use the pool allocator
260  island->id = id;
261  island->bodyArray.reserve( allocSize );
262  m_allocatedIslands.push_back( island );
263  }
264  m_lookupIslandFromId[ id ] = island;
265  if ( numBodies < m_batchIslandMinBodyCount )
266  {
267  m_batchIsland = island;
268  }
269  m_activeIslands.push_back( island );
270  return island;
271 }
272 
273 
275 {
276 
277  BT_PROFILE("islandUnionFindAndQuickSort");
278 
279  btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
280 
281  //we are going to sort the unionfind array, and store the element id in the size
282  //afterwards, we clean unionfind, to make sure no-one uses it anymore
283 
285  int numElem = getUnionFind().getNumElements();
286 
287  int endIslandIndex=1;
288  int startIslandIndex;
289 
290  //update the sleeping state for bodies, if all are sleeping
291  for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
292  {
293  int islandId = getUnionFind().getElement(startIslandIndex).m_id;
294  for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
295  {
296  }
297 
298  //int numSleeping = 0;
299 
300  bool allSleeping = true;
301 
302  int idx;
303  for (idx=startIslandIndex;idx<endIslandIndex;idx++)
304  {
305  int i = getUnionFind().getElement(idx).m_sz;
306 
307  btCollisionObject* colObj0 = collisionObjects[i];
308  if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
309  {
310 // printf("error in island management\n");
311  }
312 
313  btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
314  if (colObj0->getIslandTag() == islandId)
315  {
316  if (colObj0->getActivationState()== ACTIVE_TAG)
317  {
318  allSleeping = false;
319  }
320  if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
321  {
322  allSleeping = false;
323  }
324  }
325  }
326 
327  if (allSleeping)
328  {
329  int idx;
330  for (idx=startIslandIndex;idx<endIslandIndex;idx++)
331  {
332  int i = getUnionFind().getElement(idx).m_sz;
333  btCollisionObject* colObj0 = collisionObjects[i];
334  if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
335  {
336 // printf("error in island management\n");
337  }
338 
339  btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
340 
341  if (colObj0->getIslandTag() == islandId)
342  {
344  }
345  }
346  } else
347  {
348 
349  int idx;
350  for (idx=startIslandIndex;idx<endIslandIndex;idx++)
351  {
352  int i = getUnionFind().getElement(idx).m_sz;
353 
354  btCollisionObject* colObj0 = collisionObjects[i];
355  if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
356  {
357 // printf("error in island management\n");
358  }
359 
360  btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
361 
362  if (colObj0->getIslandTag() == islandId)
363  {
364  if ( colObj0->getActivationState() == ISLAND_SLEEPING)
365  {
367  colObj0->setDeactivationTime(0.f);
368  }
369  }
370  }
371  }
372  }
373 }
374 
375 
377 {
378  btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
379  int endIslandIndex = 1;
380  int startIslandIndex;
381  int numElem = getUnionFind().getNumElements();
382 
383  // create explicit islands and add bodies to each
384  for ( startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex )
385  {
386  int islandId = getUnionFind().getElement( startIslandIndex ).m_id;
387 
388  // find end index
389  for ( endIslandIndex = startIslandIndex; ( endIslandIndex < numElem ) && ( getUnionFind().getElement( endIslandIndex ).m_id == islandId ); endIslandIndex++ )
390  {
391  }
392  // check if island is sleeping
393  bool islandSleeping = true;
394  for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
395  {
396  int i = getUnionFind().getElement( iElem ).m_sz;
397  btCollisionObject* colObj = collisionObjects[ i ];
398  if ( colObj->isActive() )
399  {
400  islandSleeping = false;
401  }
402  }
403  if ( !islandSleeping )
404  {
405  // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures
406  int numBodies = endIslandIndex - startIslandIndex;
407  Island* island = allocateIsland( islandId, numBodies );
408  island->isSleeping = false;
409 
410  // add bodies to island
411  for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
412  {
413  int i = getUnionFind().getElement( iElem ).m_sz;
414  btCollisionObject* colObj = collisionObjects[ i ];
415  island->bodyArray.push_back( colObj );
416  }
417  }
418  }
419 
420 }
421 
422 
424 {
425  // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island
426  int maxNumManifolds = dispatcher->getNumManifolds();
427  for ( int i = 0; i < maxNumManifolds; i++ )
428  {
429  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal( i );
430 
431  const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
432  const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
433 
435  if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
436  ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
437  {
438 
439  //kinematic objects don't merge islands, but wake up all connected objects
440  if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
441  {
442  if ( colObj0->hasContactResponse() )
443  colObj1->activate();
444  }
445  if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
446  {
447  if ( colObj1->hasContactResponse() )
448  colObj0->activate();
449  }
450  //filtering for response
451  if ( dispatcher->needsResponse( colObj0, colObj1 ) )
452  {
453  // scatter manifolds into various islands
454  int islandId = getIslandId( manifold );
455  // if island not sleeping,
456  if ( Island* island = getIsland( islandId ) )
457  {
458  island->manifoldArray.push_back( manifold );
459  }
460  }
461  }
462  }
463 }
464 
465 
467 {
468  // walk constraints
469  for ( int i = 0; i < constraints.size(); i++ )
470  {
471  // scatter constraints into various islands
472  btTypedConstraint* constraint = constraints[ i ];
473  if ( constraint->isEnabled() )
474  {
475  int islandId = btGetConstraintIslandId( constraint );
476  // if island is not sleeping,
477  if ( Island* island = getIsland( islandId ) )
478  {
479  island->constraintArray.push_back( constraint );
480  }
481  }
482  }
483 }
484 
485 
487 {
488  // sort islands in order of decreasing batch size
490 
491  // merge small islands to satisfy minimum batch size
492  // find first small batch island
493  int destIslandIndex = m_activeIslands.size();
494  for ( int i = 0; i < m_activeIslands.size(); ++i )
495  {
496  Island* island = m_activeIslands[ i ];
497  int batchSize = calcBatchCost( island );
498  if ( batchSize < m_minimumSolverBatchSize )
499  {
500  destIslandIndex = i;
501  break;
502  }
503  }
504  int lastIndex = m_activeIslands.size() - 1;
505  while ( destIslandIndex < lastIndex )
506  {
507  // merge islands from the back of the list
508  Island* island = m_activeIslands[ destIslandIndex ];
509  int numBodies = island->bodyArray.size();
510  int numManifolds = island->manifoldArray.size();
511  int numConstraints = island->constraintArray.size();
512  int firstIndex = lastIndex;
513  // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have
514  while ( true )
515  {
516  Island* src = m_activeIslands[ firstIndex ];
517  numBodies += src->bodyArray.size();
518  numManifolds += src->manifoldArray.size();
519  numConstraints += src->constraintArray.size();
520  int batchCost = calcBatchCost( numBodies, numManifolds, numConstraints );
521  if ( batchCost >= m_minimumSolverBatchSize )
522  {
523  break;
524  }
525  if ( firstIndex - 1 == destIslandIndex )
526  {
527  break;
528  }
529  firstIndex--;
530  }
531  // reserve space for these pointers to minimize reallocation
532  island->bodyArray.reserve( numBodies );
533  island->manifoldArray.reserve( numManifolds );
534  island->constraintArray.reserve( numConstraints );
535  // merge islands
536  for ( int i = firstIndex; i <= lastIndex; ++i )
537  {
538  island->append( *m_activeIslands[ i ] );
539  }
540  // shrink array to exclude the islands that were merged from
541  m_activeIslands.resize( firstIndex );
542  lastIndex = firstIndex - 1;
543  destIslandIndex++;
544  }
545 }
546 
547 
549 {
550  // serial dispatch
551  btAlignedObjectArray<Island*>& islands = *islandsPtr;
552  for ( int i = 0; i < islands.size(); ++i )
553  {
554  Island* island = islands[ i ];
555  btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
556  btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
557  callback->processIsland( &island->bodyArray[ 0 ],
558  island->bodyArray.size(),
559  manifolds,
560  island->manifoldArray.size(),
561  constraintsPtr,
562  island->constraintArray.size(),
563  island->id
564  );
565  }
566 }
567 
570  btCollisionWorld* collisionWorld,
572  IslandCallback* callback
573  )
574 {
575  btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
576 
577  buildIslands(dispatcher,collisionWorld);
578 
579  BT_PROFILE("processIslands");
580 
581  if(!getSplitIslands())
582  {
583  btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
584  int maxNumManifolds = dispatcher->getNumManifolds();
585 
586  for ( int i = 0; i < maxNumManifolds; i++ )
587  {
588  btPersistentManifold* manifold = manifolds[ i ];
589 
590  const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
591  const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
592 
594  if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
595  ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
596  {
597 
598  //kinematic objects don't merge islands, but wake up all connected objects
599  if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
600  {
601  if ( colObj0->hasContactResponse() )
602  colObj1->activate();
603  }
604  if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
605  {
606  if ( colObj1->hasContactResponse() )
607  colObj0->activate();
608  }
609  }
610  }
611  btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
612  callback->processIsland(&collisionObjects[0],
613  collisionObjects.size(),
614  manifolds,
615  maxNumManifolds,
616  constraintsPtr,
617  constraints.size(),
618  -1
619  );
620  }
621  else
622  {
623  initIslandPools();
624 
625  //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
626  addBodiesToIslands( collisionWorld );
627  addManifoldsToIslands( dispatcher );
628  addConstraintsToIslands( constraints );
629 
630  // m_activeIslands array should now contain all non-sleeping Islands, and each Island should
631  // have all the necessary bodies, manifolds and constraints.
632 
633  // if we want to merge islands with small batch counts,
634  if ( m_minimumSolverBatchSize > 1 )
635  {
636  mergeIslands();
637  }
638  // dispatch islands to solver
639  m_islandDispatch( &m_activeIslands, callback );
640  }
641 }
virtual Island * allocateIsland(int id, int numBodies)
#define ACTIVE_TAG
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
void push_back(const T &_Val)
void sortIslands()
this is a special operation, destroying the content of btUnionFind.
Definition: btUnionFind.cpp:64
int getIslandId(const btPersistentManifold *lhs)
btAlignedObjectArray< btPersistentManifold * > manifoldArray
btAlignedObjectArray< Island * > m_freeIslands
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
btAlignedObjectArray< Island * > m_lookupIslandFromId
virtual btPersistentManifold * getManifoldByIndexInternal(int index)=0
virtual void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, btAlignedObjectArray< btTypedConstraint *> &constraints, IslandCallback *callback)
static void getElement(int arrayLen, const char *cur, const char *old, char *oldPtr, char *curData)
Definition: bFile.cpp:868
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, btTypedConstraint **constraints, int numConstraints, int islandId)=0
const btRigidBody & getRigidBodyA() const
bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island &island, const btCollisionObject *obj)
#define btAssert(x)
Definition: btScalar.h:114
btElement & getElement(int index)
Definition: btUnionFind.h:61
btCollisionObjectArray & getCollisionObjectArray()
virtual void addBodiesToIslands(btCollisionWorld *collisionWorld)
bool isKinematicObject() const
#define SIMD_FORCE_INLINE
Definition: btScalar.h:64
int getActivationState() const
const btRigidBody & getRigidBodyB() const
function object that routes calls to operator<
#define ISLAND_SLEEPING
static void defaultIslandDispatch(btAlignedObjectArray< Island *> *islands, IslandCallback *callback)
btAlignedObjectArray< Island * > m_activeIslands
void activate(bool forceActivation=false) const
btAlignedObjectArray< Island * > m_allocatedIslands
const btCollisionObject * getBody0() const
bool isEnabled() const
int btGetConstraintIslandId(const btTypedConstraint *lhs)
btCollisionObject can be used to manage collision detection objects.
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
bool hasContactResponse() const
btAlignedObjectArray< btTypedConstraint * > constraintArray
void setDeactivationTime(btScalar time)
virtual btPersistentManifold ** getInternalManifoldPointer()=0
int size() const
return the number of elements in the array
virtual int getNumManifolds() const =0
#define BT_PROFILE(name)
Definition: btQuickprof.h:213
CollisionWorld is interface and container for the collision detection.
int getIslandTag() const
#define WANTS_DEACTIVATION
virtual void addConstraintsToIslands(btAlignedObjectArray< btTypedConstraint *> &constraints)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void resize(int newsize, const T &fillData=T())
int calcBatchCost(int bodies, int manifolds, int constraints)
const btCollisionObject * getBody1() const
int getNumElements() const
Definition: btUnionFind.h:52
virtual void buildIslands(btDispatcher *dispatcher, btCollisionWorld *colWorld)
virtual bool needsResponse(const btCollisionObject *body0, const btCollisionObject *body1)=0
#define DISABLE_DEACTIVATION
btAlignedObjectArray< btCollisionObject * > bodyArray
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:75
virtual void addManifoldsToIslands(btDispatcher *dispatcher)
void setActivationState(int newState) const
void quickSort(const L &CompareFunc)