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  if (colObj0->getIslandTag() == islandId)
284  {
285  if (colObj0->getActivationState() == ISLAND_SLEEPING)
286  {
288  colObj0->setDeactivationTime(0.f);
289  }
290  }
291  }
292  }
293  }
294 
295  int i;
296  int maxNumManifolds = dispatcher->getNumManifolds();
297 
298  //#define SPLIT_ISLANDS 1
299  //#ifdef SPLIT_ISLANDS
300 
301  //#endif //SPLIT_ISLANDS
302 
303  for (i = 0; i < maxNumManifolds; i++)
304  {
305  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
306  if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
307  {
308  if (manifold->getNumContacts() == 0)
309  continue;
310  }
311 
312  const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
313  const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
314 
316  if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
317  ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
318  {
319  //kinematic objects don't merge islands, but wake up all connected objects
320  if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
321  {
322  if (colObj0->hasContactResponse())
323  colObj1->activate();
324  }
325  if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
326  {
327  if (colObj1->hasContactResponse())
328  colObj0->activate();
329  }
330  if (m_splitIslands)
331  {
332  //filtering for response
333  if (dispatcher->needsResponse(colObj0, colObj1))
334  m_islandmanifold.push_back(manifold);
335  }
336  }
337  }
338 }
339 
342 {
343  btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
344 
345  buildIslands(dispatcher, collisionWorld);
346 
347  int endIslandIndex = 1;
348  int startIslandIndex;
349  int numElem = getUnionFind().getNumElements();
350 
351  BT_PROFILE("processIslands");
352 
353  if (!m_splitIslands)
354  {
355  btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
356  int maxNumManifolds = dispatcher->getNumManifolds();
357  callback->processIsland(&collisionObjects[0], collisionObjects.size(), manifold, maxNumManifolds, -1);
358  }
359  else
360  {
361  // Sort manifolds, based on islands
362  // Sort the vector using predicate and std::sort
363  //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
364 
365  int numManifolds = int(m_islandmanifold.size());
366 
367  //tried a radix sort, but quicksort/heapsort seems still faster
368  //@todo rewrite island management
369  //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
370  //but also based on object0 unique id and object1 unique id
371  if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
372  {
374  }
375  else
376  {
378  }
379 
380  //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
381 
382  //now process all active islands (sets of manifolds for now)
383 
384  int startManifoldIndex = 0;
385  int endManifoldIndex = 1;
386 
387  //int islandId;
388 
389  // printf("Start Islands\n");
390 
391  //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
392  for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
393  {
394  int islandId = getUnionFind().getElement(startIslandIndex).m_id;
395 
396  bool islandSleeping = true;
397 
398  for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
399  {
400  int i = getUnionFind().getElement(endIslandIndex).m_sz;
401  btCollisionObject* colObj0 = collisionObjects[i];
402  m_islandBodies.push_back(colObj0);
403  if (colObj0->isActive())
404  islandSleeping = false;
405  }
406 
407  //find the accompanying contact manifold for this islandId
408  int numIslandManifolds = 0;
409  btPersistentManifold** startManifold = 0;
410 
411  if (startManifoldIndex < numManifolds)
412  {
413  int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
414  if (curIslandId == islandId)
415  {
416  startManifold = &m_islandmanifold[startManifoldIndex];
417 
418  for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
419  {
420  }
422  numIslandManifolds = endManifoldIndex - startManifoldIndex;
423  }
424  }
425 
426  if (!islandSleeping)
427  {
428  callback->processIsland(&m_islandBodies[0], m_islandBodies.size(), startManifold, numIslandManifolds, islandId);
429  // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
430  }
431 
432  if (numIslandManifolds)
433  {
434  startManifoldIndex = endManifoldIndex;
435  }
436 
438  }
439  } // else if(!splitIslands)
440 }
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:341
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
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:348
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:275
btBroadphaseProxy::m_uniqueId
int m_uniqueId
Definition: btBroadphaseProxy.h:106
btUnionFind::getElement
btElement & getElement(int index)
Definition: btUnionFind.h:59
btScalar.h
btCollisionObject::setHitFraction
void setHitFraction(btScalar hitFraction)
Definition: btCollisionObject.h:470
btCollisionObject::isKinematicObject
bool isKinematicObject() const
Definition: btCollisionObject.h:198
btCollisionWorld.h
btSimulationIslandManager::m_unionFind
btUnionFind m_unionFind
Definition: btSimulationIslandManager.h:32
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:203
ISLAND_SLEEPING
#define ISLAND_SLEEPING
Definition: btCollisionObject.h:23
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btCollisionObject::activate
void activate(bool forceActivation=false) const
Definition: btCollisionObject.cpp:71
btSimulationIslandManager::m_islandBodies
btAlignedObjectArray< btCollisionObject * > m_islandBodies
Definition: btSimulationIslandManager.h:35
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:210
btSimulationIslandManager::m_islandmanifold
btAlignedObjectArray< btPersistentManifold * > m_islandmanifold
Definition: btSimulationIslandManager.h:34
btBroadphasePair::m_pProxy1
btBroadphaseProxy * m_pProxy1
Definition: btBroadphaseProxy.h:216
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:381
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:449
btCollisionObject::setDeactivationTime
void setDeactivationTime(btScalar time)
Definition: btCollisionObject.h:279
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:215
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:208
btDispatcherInfo::m_deterministicOverlappingPairs
bool m_deterministicOverlappingPairs
Definition: btDispatcher.h:65
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:434
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:52
btElement::m_id
int m_id
Definition: btUnionFind.h:28
btDispatcher.h
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:83
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:439
btCollisionObject::isActive
bool isActive() const
Definition: btCollisionObject.h:292
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:264
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:60
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btDispatcher::getManifoldByIndexInternal
virtual btPersistentManifold * getManifoldByIndexInternal(int index)=0