Bullet Collision Detection & Physics Library: btSimulationIslandManager.cpp Source File

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