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 }
static void getElement(int arrayLen, const char *cur, const char *old, char *oldPtr, char *curData)
Definition: bFile.cpp:816
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
#define ISLAND_SLEEPING
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
#define btAssert(x)
Definition: btScalar.h:153
int getIslandId(const btPersistentManifold *lhs)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
void push_back(const T &_Val)
btCollisionObject can be used to manage collision detection objects.
bool isStaticOrKinematicObject() const
btBroadphaseProxy * getBroadphaseHandle()
bool hasContactResponse() const
void activate(bool forceActivation=false) const
void setActivationState(int newState) const
void setCompanionId(int id)
bool isKinematicObject() const
int getIslandTag() const
void setDeactivationTime(btScalar time)
void setIslandTag(int tag)
void setHitFraction(btScalar hitFraction)
int getActivationState() const
CollisionWorld is interface and container for the collision detection.
btCollisionObjectArray & getCollisionObjectArray()
btOverlappingPairCache * getPairCache()
btDispatcherInfo & getDispatchInfo()
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
virtual int getNumManifolds() const =0
virtual btPersistentManifold * getManifoldByIndexInternal(int index)=0
virtual btPersistentManifold ** getInternalManifoldPointer()=0
virtual bool needsResponse(const btCollisionObject *body0, const btCollisionObject *body1)=0
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
virtual int getNumOverlappingPairs() const =0
virtual btBroadphasePair * getOverlappingPairArrayPtr()=0
bool operator()(const btPersistentManifold *lhs, const btPersistentManifold *rhs) const
function object that routes calls to operator<
bool operator()(const btPersistentManifold *lhs, const btPersistentManifold *rhs) const
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
const btCollisionObject * getBody1() const
const btCollisionObject * getBody0() const
virtual void storeIslandActivationState(btCollisionWorld *world)
void findUnions(btDispatcher *dispatcher, btCollisionWorld *colWorld)
void processIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
btAlignedObjectArray< btCollisionObject * > m_islandBodies
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
btAlignedObjectArray< btPersistentManifold * > m_islandmanifold
void buildIslands(btDispatcher *dispatcher, btCollisionWorld *colWorld)
void reset(int N)
Definition: btUnionFind.cpp:36
int getNumElements() const
Definition: btUnionFind.h:50
void unite(int p, int q)
Definition: btUnionFind.h:76
void sortIslands()
this is a special operation, destroying the content of btUnionFind.
Definition: btUnionFind.cpp:58
btElement & getElement(int index)
Definition: btUnionFind.h:59
int find(int p, int q)
Definition: btUnionFind.h:71
The btBroadphasePair class contains a pair of aabb-overlapping objects.
btBroadphaseProxy * m_pProxy1
btBroadphaseProxy * m_pProxy0
bool m_deterministicOverlappingPairs
Definition: btDispatcher.h:65
virtual void processIsland(btCollisionObject **bodies, int numBodies, class btPersistentManifold **manifolds, int numManifolds, int islandId)=0