Blender  V3.3
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 (
192  (getIslandId(lhs) < getIslandId(rhs)) || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) || ((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId)));
193  }
194 };
195 
197 {
198  BT_PROFILE("islandUnionFindAndQuickSort");
199 
200  btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
201 
202  m_islandmanifold.resize(0);
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 ||
240  colObj0->getActivationState() == DISABLE_DEACTIVATION)
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  {
264  colObj0->setActivationState(ISLAND_SLEEPING);
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  {
288  colObj0->setActivationState(WANTS_DEACTIVATION);
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  {
382  m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
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 
442  m_islandBodies.resize(0);
443  }
444  } // else if(!splitIslands)
445 }
btBroadphasePair
btCollisionObject
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
SIMD_FORCE_INLINE bool mergesSimulationIslands() const
SIMD_FORCE_INLINE int getIslandTag() const
#define ISLAND_SLEEPING
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btPersistentManifold()
#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:280
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
int getIslandId(const btPersistentManifold *lhs)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
SIMD_FORCE_INLINE void push_back(const T &_Val)
CollisionWorld is interface and container for the collision detection.
btCollisionObjectArray & getCollisionObjectArray()
btOverlappingPairCache * getPairCache()
btDispatcherInfo & getDispatchInfo()
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
virtual int getNumOverlappingPairs() const =0
virtual btBroadphasePair * getOverlappingPairArrayPtr()=0
SIMD_FORCE_INLINE bool operator()(const btPersistentManifold *lhs, const btPersistentManifold *rhs) const
function object that routes calls to operator<
SIMD_FORCE_INLINE bool operator()(const btPersistentManifold *lhs, const btPersistentManifold *rhs) 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)
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
void buildIslands(btDispatcher *dispatcher, btCollisionWorld *colWorld)
void reset(int N)
Definition: btUnionFind.cpp:36
void unite(int p, int q)
Definition: btUnionFind.h:76
SIMD_FORCE_INLINE int getNumElements() const
Definition: btUnionFind.h:50
void sortIslands()
Definition: btUnionFind.cpp:58
btElement & getElement(int index)
Definition: btUnionFind.h:59
int find(int p, int q)
Definition: btUnionFind.h:71
DEGForeachIDComponentCallback callback
bool m_deterministicOverlappingPairs
Definition: btDispatcher.h:65