Bullet Collision Detection & Physics Library
btSimulationIslandManager.cpp
Go to the documentation of this file.
1 
2 /*
3 Bullet Continuous Collision Detection and Physics Library
4 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
5 
6 This software is provided 'as-is', without any express or implied warranty.
7 In no event will the authors be held liable for any damages arising from the use of this software.
8 Permission is granted to anyone to use this software for any purpose,
9 including commercial applications, and to alter it and redistribute it freely,
10 subject to the following restrictions:
11 
12 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
13 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
14 3. This notice may not be removed or altered from any source distribution.
15 */
16 
17 #include "LinearMath/btScalar.h"
23 
24 //#include <stdio.h>
25 #include "LinearMath/btQuickprof.h"
26 
28 {
29 }
30 
32 {
33 }
34 
36 {
37  m_unionFind.reset(n);
38 }
39 
41 {
42  {
43  btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
44  const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
45  if (numOverlappingPairs)
46  {
47  btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
48 
49  for (int i = 0; i < numOverlappingPairs; i++)
50  {
51  const btBroadphasePair& collisionPair = pairPtr[i];
52  btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
53  btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
54 
55  if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
56  ((colObj1) && ((colObj1)->mergesSimulationIslands())))
57  {
58  m_unionFind.unite((colObj0)->getIslandTag(),
59  (colObj1)->getIslandTag());
60  }
61  }
62  }
63  }
64 }
65 
66 #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
68 {
69  // put the index into m_controllers into m_tag
70  int index = 0;
71  {
72  int i;
73  for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
74  {
75  btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
76  //Adding filtering here
77  if (!collisionObject->isStaticOrKinematicObject())
78  {
79  collisionObject->setIslandTag(index++);
80  }
81  collisionObject->setCompanionId(-1);
82  collisionObject->setHitFraction(btScalar(1.));
83  }
84  }
85  // do the union find
86 
87  initUnionFind(index);
88 
89  findUnions(dispatcher, colWorld);
90 }
91 
93 {
94  // put the islandId ('find' value) into m_tag
95  {
96  int index = 0;
97  int i;
98  for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
99  {
100  btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
101  if (!collisionObject->isStaticOrKinematicObject())
102  {
103  collisionObject->setIslandTag(m_unionFind.find(index));
104  //Set the correct object offset in Collision Object Array
105  m_unionFind.getElement(index).m_sz = i;
106  collisionObject->setCompanionId(-1);
107  index++;
108  }
109  else
110  {
111  collisionObject->setIslandTag(-1);
112  collisionObject->setCompanionId(-2);
113  }
114  }
115  }
116 }
117 
118 #else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
120 {
121  initUnionFind(int(colWorld->getCollisionObjectArray().size()));
122 
123  // put the index into m_controllers into m_tag
124  {
125  int index = 0;
126  int i;
127  for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
128  {
129  btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
130  collisionObject->setIslandTag(index);
131  collisionObject->setCompanionId(-1);
132  collisionObject->setHitFraction(btScalar(1.));
133  index++;
134  }
135  }
136  // do the union find
137 
138  findUnions(dispatcher, colWorld);
139 }
140 
142 {
143  // put the islandId ('find' value) into m_tag
144  {
145  int index = 0;
146  int i;
147  for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
148  {
149  btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
150  if (!collisionObject->isStaticOrKinematicObject())
151  {
152  collisionObject->setIslandTag(m_unionFind.find(index));
153  collisionObject->setCompanionId(-1);
154  }
155  else
156  {
157  collisionObject->setIslandTag(-1);
158  collisionObject->setCompanionId(-2);
159  }
160  index++;
161  }
162  }
163 }
164 
165 #endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
166 
167 inline int getIslandId(const btPersistentManifold* lhs)
168 {
169  int islandId;
170  const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
171  const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
172  islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
173  return islandId;
174 }
175 
178 {
179 public:
181  {
182  return getIslandId(lhs) < getIslandId(rhs);
183  }
184 };
185 
187 {
188 public:
190  {
191  return (
193  }
194 };
195 
197 {
198  BT_PROFILE("islandUnionFindAndQuickSort");
199 
200  btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
201 
203 
204  //we are going to sort the unionfind array, and store the element id in the size
205  //afterwards, we clean unionfind, to make sure no-one uses it anymore
206 
208  int numElem = getUnionFind().getNumElements();
209 
210  int endIslandIndex = 1;
211  int startIslandIndex;
212 
213  //update the sleeping state for bodies, if all are sleeping
214  for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
215  {
216  int islandId = getUnionFind().getElement(startIslandIndex).m_id;
217  for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
218  {
219  }
220 
221  //int numSleeping = 0;
222 
223  bool allSleeping = true;
224 
225  int idx;
226  for (idx = startIslandIndex; idx < endIslandIndex; idx++)
227  {
228  int i = getUnionFind().getElement(idx).m_sz;
229 
230  btCollisionObject* colObj0 = collisionObjects[i];
231  if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
232  {
233  // printf("error in island management\n");
234  }
235 
236  btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
237  if (colObj0->getIslandTag() == islandId)
238  {
239  if (colObj0->getActivationState() == ACTIVE_TAG ||
241  {
242  allSleeping = false;
243  break;
244  }
245  }
246  }
247 
248  if (allSleeping)
249  {
250  int idx;
251  for (idx = startIslandIndex; idx < endIslandIndex; idx++)
252  {
253  int i = getUnionFind().getElement(idx).m_sz;
254  btCollisionObject* colObj0 = collisionObjects[i];
255  if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
256  {
257  // printf("error in island management\n");
258  }
259 
260  btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
261 
262  if (colObj0->getIslandTag() == islandId)
263  {
265  }
266  }
267  }
268  else
269  {
270  int idx;
271  for (idx = startIslandIndex; idx < endIslandIndex; idx++)
272  {
273  int i = getUnionFind().getElement(idx).m_sz;
274 
275  btCollisionObject* colObj0 = collisionObjects[i];
276  if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
277  {
278  // printf("error in island management\n");
279  }
280 
281  btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
282 
283 
284  if (colObj0->getIslandTag() == islandId)
285  {
286  if (colObj0->getActivationState() == ISLAND_SLEEPING)
287  {
289  colObj0->setDeactivationTime(0.f);
290  }
291  }
292  }
293  }
294  }
295 
296  int i;
297  int maxNumManifolds = dispatcher->getNumManifolds();
298 
299  //#define SPLIT_ISLANDS 1
300  //#ifdef SPLIT_ISLANDS
301 
302  //#endif //SPLIT_ISLANDS
303 
304  for (i = 0; i < maxNumManifolds; i++)
305  {
306  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
307  if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
308  {
309  if (manifold->getNumContacts() == 0)
310  continue;
311  }
312 
313  const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
314  const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
315 
317  if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
318  ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
319  {
320  //kinematic objects don't merge islands, but wake up all connected objects
321  if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
322  {
323  if (colObj0->hasContactResponse())
324  colObj1->activate();
325  }
326  if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
327  {
328  if (colObj1->hasContactResponse())
329  colObj0->activate();
330  }
331  if (m_splitIslands)
332  {
333  //filtering for response
334  if (dispatcher->needsResponse(colObj0, colObj1))
335  m_islandmanifold.push_back(manifold);
336  }
337  }
338  }
339 }
340 
341 
344 {
345  buildIslands(dispatcher, collisionWorld);
346  processIslands(dispatcher, collisionWorld, callback);
347 }
348 
350 {
351  btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
352  int endIslandIndex = 1;
353  int startIslandIndex;
354  int numElem = getUnionFind().getNumElements();
355 
356  BT_PROFILE("processIslands");
357 
358  if (!m_splitIslands)
359  {
360  btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
361  int maxNumManifolds = dispatcher->getNumManifolds();
362  callback->processIsland(&collisionObjects[0], collisionObjects.size(), manifold, maxNumManifolds, -1);
363  }
364  else
365  {
366  // Sort manifolds, based on islands
367  // Sort the vector using predicate and std::sort
368  //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
369 
370  int numManifolds = int(m_islandmanifold.size());
371 
372  //tried a radix sort, but quicksort/heapsort seems still faster
373  //@todo rewrite island management
374  //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
375  //but also based on object0 unique id and object1 unique id
376  if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
377  {
379  }
380  else
381  {
383  }
384 
385  //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
386 
387  //now process all active islands (sets of manifolds for now)
388 
389  int startManifoldIndex = 0;
390  int endManifoldIndex = 1;
391 
392  //int islandId;
393 
394  // printf("Start Islands\n");
395 
396  //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
397  for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
398  {
399  int islandId = getUnionFind().getElement(startIslandIndex).m_id;
400 
401  bool islandSleeping = true;
402 
403  for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
404  {
405  int i = getUnionFind().getElement(endIslandIndex).m_sz;
406  btCollisionObject* colObj0 = collisionObjects[i];
407  m_islandBodies.push_back(colObj0);
408  if (colObj0->isActive())
409  islandSleeping = false;
410  }
411 
412  //find the accompanying contact manifold for this islandId
413  int numIslandManifolds = 0;
414  btPersistentManifold** startManifold = 0;
415 
416  if (startManifoldIndex < numManifolds)
417  {
418  int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
419  if (curIslandId == islandId)
420  {
421  startManifold = &m_islandmanifold[startManifoldIndex];
422 
423  for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
424  {
425  }
427  numIslandManifolds = endManifoldIndex - startManifoldIndex;
428  }
429  }
430 
431  if (!islandSleeping)
432  {
433  callback->processIsland(&m_islandBodies[0], m_islandBodies.size(), startManifold, numIslandManifolds, islandId);
434  // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
435  }
436 
437  if (numIslandManifolds)
438  {
439  startManifoldIndex = endManifoldIndex;
440  }
441 
443  }
444  } // else if(!splitIslands)
445 }
getElement
static void getElement(int arrayLen, const char *cur, const char *old, char *oldPtr, char *curData)
Definition: bFile.cpp:816
btSimulationIslandManager.h
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:48
btUnionFind::unite
void unite(int p, int q)
Definition: btUnionFind.h:76
btSimulationIslandManager::storeIslandActivationState
virtual void storeIslandActivationState(btCollisionWorld *world)
Definition: btSimulationIslandManager.cpp:141
btElement::m_sz
int m_sz
Definition: btUnionFind.h:29
DISABLE_DEACTIVATION
#define DISABLE_DEACTIVATION
Definition: btCollisionObject.h:25
btSimulationIslandManager::findUnions
void findUnions(btDispatcher *dispatcher, btCollisionWorld *colWorld)
Definition: btSimulationIslandManager.cpp:40
btDispatcher::getInternalManifoldPointer
virtual btPersistentManifold ** getInternalManifoldPointer()=0
btSimulationIslandManager::buildAndProcessIslands
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
Definition: btSimulationIslandManager.cpp:343
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
ACTIVE_TAG
#define ACTIVE_TAG
Definition: btCollisionObject.h:22
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:76
getIslandId
int getIslandId(const btPersistentManifold *lhs)
Definition: btSimulationIslandManager.cpp:167
btAlignedObjectArray::quickSort
void quickSort(const L &CompareFunc)
Definition: btAlignedObjectArray.h:341
btSimulationIslandManager::buildIslands
void buildIslands(btDispatcher *dispatcher, btCollisionWorld *colWorld)
Definition: btSimulationIslandManager.cpp:196
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:105
btDispatcher::getNumManifolds
virtual int getNumManifolds() const =0
btUnionFind::reset
void reset(int N)
Definition: btUnionFind.cpp:36
btOverlappingPairCache::getNumOverlappingPairs
virtual int getNumOverlappingPairs() const =0
btPersistentManifoldSortPredicate
function object that routes calls to operator<
Definition: btSimulationIslandManager.cpp:177
btUnionFind::sortIslands
void sortIslands()
this is a special operation, destroying the content of btUnionFind.
Definition: btUnionFind.cpp:58
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:120
btCollisionObject::getActivationState
int getActivationState() const
Definition: btCollisionObject.h:277
btBroadphaseProxy::m_uniqueId
int m_uniqueId
Definition: btBroadphaseProxy.h:106
btUnionFind::getElement
btElement & getElement(int index)
Definition: btUnionFind.h:59
btSimulationIslandManager::processIslands
void processIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
Definition: btSimulationIslandManager.cpp:349
btScalar.h
btCollisionObject::setHitFraction
void setHitFraction(btScalar hitFraction)
Definition: btCollisionObject.h:472
btCollisionObject::isKinematicObject
bool isKinematicObject() const
Definition: btCollisionObject.h:200
btCollisionWorld.h
btSimulationIslandManager::m_unionFind
btUnionFind m_unionFind
Definition: btSimulationIslandManager.h:32
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:205
ISLAND_SLEEPING
#define ISLAND_SLEEPING
Definition: btCollisionObject.h:23
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btCollisionObject::activate
void activate(bool forceActivation=false) const
Definition: btCollisionObject.cpp:72
btSimulationIslandManager::m_islandBodies
btAlignedObjectArray< btCollisionObject * > m_islandBodies
Definition: btSimulationIslandManager.h:35
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btSimulationIslandManager::m_islandmanifold
btAlignedObjectArray< btPersistentManifold * > m_islandmanifold
Definition: btSimulationIslandManager.h:34
btBroadphasePair::m_pProxy1
btBroadphaseProxy * m_pProxy1
Definition: btBroadphaseProxy.h:209
btBroadphaseProxy::m_clientObject
void * m_clientObject
Definition: btBroadphaseProxy.h:102
btCollisionWorld::getPairCache
btOverlappingPairCache * getPairCache()
Definition: btCollisionWorld.h:127
btSimulationIslandManager::initUnionFind
void initUnionFind(int n)
Definition: btSimulationIslandManager.cpp:35
btOverlappingPairCache
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
Definition: btOverlappingPairCache.h:50
btCollisionObject::getBroadphaseHandle
btBroadphaseProxy * getBroadphaseHandle()
Definition: btCollisionObject.h:383
btPersistentManifoldSortPredicate::operator()
bool operator()(const btPersistentManifold *lhs, const btPersistentManifold *rhs) const
Definition: btSimulationIslandManager.cpp:180
btOverlappingPairCache::getOverlappingPairArrayPtr
virtual btBroadphasePair * getOverlappingPairArrayPtr()=0
btCollisionObject::setCompanionId
void setCompanionId(int id)
Definition: btCollisionObject.h:451
btCollisionObject::setDeactivationTime
void setDeactivationTime(btScalar time)
Definition: btCollisionObject.h:281
btPersistentManifoldSortPredicateDeterministic
Definition: btSimulationIslandManager.cpp:186
btDispatcher::needsResponse
virtual bool needsResponse(const btCollisionObject *body0, const btCollisionObject *body1)=0
btSimulationIslandManager::IslandCallback
Definition: btSimulationIslandManager.h:52
btUnionFind::getNumElements
int getNumElements() const
Definition: btUnionFind.h:50
btSimulationIslandManager::updateActivationState
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
Definition: btSimulationIslandManager.cpp:119
btBroadphasePair::m_pProxy0
btBroadphaseProxy * m_pProxy0
Definition: btBroadphaseProxy.h:208
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:63
btCollisionObject::hasContactResponse
bool hasContactResponse() const
Definition: btCollisionObject.h:210
btDispatcherInfo::m_deterministicOverlappingPairs
bool m_deterministicOverlappingPairs
Definition: btDispatcher.h:65
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:436
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:45
btElement::m_id
int m_id
Definition: btUnionFind.h:28
btDispatcher.h
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
btPersistentManifold.h
btPersistentManifoldSortPredicateDeterministic::operator()
bool operator()(const btPersistentManifold *lhs, const btPersistentManifold *rhs) const
Definition: btSimulationIslandManager.cpp:189
btSimulationIslandManager::~btSimulationIslandManager
virtual ~btSimulationIslandManager()
Definition: btSimulationIslandManager.cpp:31
btQuickprof.h
btCollisionObject::setIslandTag
void setIslandTag(int tag)
Definition: btCollisionObject.h:441
btCollisionObject::isActive
bool isActive() const
Definition: btCollisionObject.h:294
btSimulationIslandManager::btSimulationIslandManager
btSimulationIslandManager()
Definition: btSimulationIslandManager.cpp:27
WANTS_DEACTIVATION
#define WANTS_DEACTIVATION
Definition: btCollisionObject.h:24
btSimulationIslandManager::IslandCallback::processIsland
virtual void processIsland(btCollisionObject **bodies, int numBodies, class btPersistentManifold **manifolds, int numManifolds, int islandId)=0
btCollisionObject.h
btSimulationIslandManager::m_splitIslands
bool m_splitIslands
Definition: btSimulationIslandManager.h:37
btCollisionWorld
CollisionWorld is interface and container for the collision detection.
Definition: btCollisionWorld.h:85
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
btCollisionWorld::getCollisionObjectArray
btCollisionObjectArray & getCollisionObjectArray()
Definition: btCollisionWorld.h:476
btSimulationIslandManager::getUnionFind
btUnionFind & getUnionFind()
Definition: btSimulationIslandManager.h:45
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btUnionFind::find
int find(int p, int q)
Definition: btUnionFind.h:71
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:197
btCollisionWorld::getDispatchInfo
btDispatcherInfo & getDispatchInfo()
Definition: btCollisionWorld.h:490
btBroadphasePair
The btBroadphasePair class contains a pair of aabb-overlapping objects.
Definition: btBroadphaseProxy.h:177
btCollisionObject::setActivationState
void setActivationState(int newState) const
Definition: btCollisionObject.cpp:61
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btDispatcher::getManifoldByIndexInternal
virtual btPersistentManifold * getManifoldByIndexInternal(int index)=0