1
2 /*
3 Bullet Continuous Collision Detection and Physics Library
4 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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"
18 #include "btSimulationIslandManager.h"
19 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
20 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
23
24 //#include <stdio.h>
25 #include "LinearMath/btQuickprof.h"
26
btSimulationIslandManager()27 btSimulationIslandManager::btSimulationIslandManager() : m_splitIslands(true)
28 {
29 }
30
~btSimulationIslandManager()31 btSimulationIslandManager::~btSimulationIslandManager()
32 {
33 }
34
initUnionFind(int n)35 void btSimulationIslandManager::initUnionFind(int n)
36 {
37 m_unionFind.reset(n);
38 }
39
findUnions(btDispatcher *,btCollisionWorld * colWorld)40 void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */, btCollisionWorld* colWorld)
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
updateActivationState(btCollisionWorld * colWorld,btDispatcher * dispatcher)67 void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher)
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
storeIslandActivationState(btCollisionWorld * colWorld)92 void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
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
updateActivationState(btCollisionWorld * colWorld,btDispatcher * dispatcher)119 void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher)
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
storeIslandActivationState(btCollisionWorld * colWorld)141 void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
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
getIslandId(const btPersistentManifold * lhs)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
176 /// function object that routes calls to operator<
177 class btPersistentManifoldSortPredicate
178 {
179 public:
operator ()(const btPersistentManifold * lhs,const btPersistentManifold * rhs) const180 SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
181 {
182 return getIslandId(lhs) < getIslandId(rhs);
183 }
184 };
185
186 class btPersistentManifoldSortPredicateDeterministic
187 {
188 public:
operator ()(const btPersistentManifold * lhs,const btPersistentManifold * rhs) const189 SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
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
buildIslands(btDispatcher * dispatcher,btCollisionWorld * collisionWorld)196 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld)
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
207 getUnionFind().sortIslands();
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
316 ///@todo: check sleeping conditions!
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
342 ///@todo: this is random access, it can be walked 'cache friendly'!
buildAndProcessIslands(btDispatcher * dispatcher,btCollisionWorld * collisionWorld,IslandCallback * callback)343 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
344 {
345 buildIslands(dispatcher, collisionWorld);
346 processIslands(dispatcher, collisionWorld, callback);
347 }
348
processIslands(btDispatcher * dispatcher,btCollisionWorld * collisionWorld,IslandCallback * callback)349 void btSimulationIslandManager::processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
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 {
378 m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
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 }
426 /// Process the actual simulation, only if not sleeping/deactivated
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 }
446