Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "LinearMath/btScalar.h"
00019 #include "btSimulationIslandManager.h"
00020 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
00022 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00023 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
00024
00025
00026 #include "LinearMath/btQuickprof.h"
00027
00028 btSimulationIslandManager::btSimulationIslandManager():
00029 m_splitIslands(true)
00030 {
00031 }
00032
00033 btSimulationIslandManager::~btSimulationIslandManager()
00034 {
00035 }
00036
00037
00038 void btSimulationIslandManager::initUnionFind(int n)
00039 {
00040 m_unionFind.reset(n);
00041 }
00042
00043
00044 void btSimulationIslandManager::findUnions(btDispatcher* ,btCollisionWorld* colWorld)
00045 {
00046
00047 {
00048 btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
00049 const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
00050 if (numOverlappingPairs)
00051 {
00052 btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
00053
00054 for (int i=0;i<numOverlappingPairs;i++)
00055 {
00056 const btBroadphasePair& collisionPair = pairPtr[i];
00057 btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
00058 btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
00059
00060 if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
00061 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
00062 {
00063
00064 m_unionFind.unite((colObj0)->getIslandTag(),
00065 (colObj1)->getIslandTag());
00066 }
00067 }
00068 }
00069 }
00070 }
00071
00072 #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
00073 void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
00074 {
00075
00076
00077 int index = 0;
00078 {
00079
00080 int i;
00081 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
00082 {
00083 btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
00084
00085 if (!collisionObject->isStaticOrKinematicObject())
00086 {
00087 collisionObject->setIslandTag(index++);
00088 }
00089 collisionObject->setCompanionId(-1);
00090 collisionObject->setHitFraction(btScalar(1.));
00091 }
00092 }
00093
00094
00095 initUnionFind( index );
00096
00097 findUnions(dispatcher,colWorld);
00098 }
00099
00100 void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
00101 {
00102
00103 {
00104 int index = 0;
00105 int i;
00106 for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
00107 {
00108 btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
00109 if (!collisionObject->isStaticOrKinematicObject())
00110 {
00111 collisionObject->setIslandTag( m_unionFind.find(index) );
00112
00113 m_unionFind.getElement(index).m_sz = i;
00114 collisionObject->setCompanionId(-1);
00115 index++;
00116 } else
00117 {
00118 collisionObject->setIslandTag(-1);
00119 collisionObject->setCompanionId(-2);
00120 }
00121 }
00122 }
00123 }
00124
00125
00126 #else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
00127 void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
00128 {
00129
00130 initUnionFind( int (colWorld->getCollisionObjectArray().size()));
00131
00132
00133 {
00134
00135 int index = 0;
00136 int i;
00137 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
00138 {
00139 btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
00140 collisionObject->setIslandTag(index);
00141 collisionObject->setCompanionId(-1);
00142 collisionObject->setHitFraction(btScalar(1.));
00143 index++;
00144
00145 }
00146 }
00147
00148
00149 findUnions(dispatcher,colWorld);
00150 }
00151
00152 void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
00153 {
00154
00155 {
00156
00157
00158 int index = 0;
00159 int i;
00160 for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
00161 {
00162 btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
00163 if (!collisionObject->isStaticOrKinematicObject())
00164 {
00165 collisionObject->setIslandTag( m_unionFind.find(index) );
00166 collisionObject->setCompanionId(-1);
00167 } else
00168 {
00169 collisionObject->setIslandTag(-1);
00170 collisionObject->setCompanionId(-2);
00171 }
00172 index++;
00173 }
00174 }
00175 }
00176
00177 #endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
00178
00179 inline int getIslandId(const btPersistentManifold* lhs)
00180 {
00181 int islandId;
00182 const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
00183 const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
00184 islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
00185 return islandId;
00186
00187 }
00188
00189
00190
00192 class btPersistentManifoldSortPredicate
00193 {
00194 public:
00195
00196 SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const
00197 {
00198 return getIslandId(lhs) < getIslandId(rhs);
00199 }
00200 };
00201
00202
00203 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
00204 {
00205
00206 BT_PROFILE("islandUnionFindAndQuickSort");
00207
00208 btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
00209
00210 m_islandmanifold.resize(0);
00211
00212
00213
00214
00215 getUnionFind().sortIslands();
00216 int numElem = getUnionFind().getNumElements();
00217
00218 int endIslandIndex=1;
00219 int startIslandIndex;
00220
00221
00222
00223 for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
00224 {
00225 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
00226 for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
00227 {
00228 }
00229
00230
00231
00232 bool allSleeping = true;
00233
00234 int idx;
00235 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00236 {
00237 int i = getUnionFind().getElement(idx).m_sz;
00238
00239 btCollisionObject* colObj0 = collisionObjects[i];
00240 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00241 {
00242
00243 }
00244
00245 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00246 if (colObj0->getIslandTag() == islandId)
00247 {
00248 if (colObj0->getActivationState()== ACTIVE_TAG)
00249 {
00250 allSleeping = false;
00251 }
00252 if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
00253 {
00254 allSleeping = false;
00255 }
00256 }
00257 }
00258
00259
00260 if (allSleeping)
00261 {
00262 int idx;
00263 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00264 {
00265 int i = getUnionFind().getElement(idx).m_sz;
00266 btCollisionObject* colObj0 = collisionObjects[i];
00267 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00268 {
00269
00270 }
00271
00272 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00273
00274 if (colObj0->getIslandTag() == islandId)
00275 {
00276 colObj0->setActivationState( ISLAND_SLEEPING );
00277 }
00278 }
00279 } else
00280 {
00281
00282 int idx;
00283 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00284 {
00285 int i = getUnionFind().getElement(idx).m_sz;
00286
00287 btCollisionObject* colObj0 = collisionObjects[i];
00288 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00289 {
00290
00291 }
00292
00293 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00294
00295 if (colObj0->getIslandTag() == islandId)
00296 {
00297 if ( colObj0->getActivationState() == ISLAND_SLEEPING)
00298 {
00299 colObj0->setActivationState( WANTS_DEACTIVATION);
00300 colObj0->setDeactivationTime(0.f);
00301 }
00302 }
00303 }
00304 }
00305 }
00306
00307
00308 int i;
00309 int maxNumManifolds = dispatcher->getNumManifolds();
00310
00311
00312
00313
00314
00315
00316
00317
00318 for (i=0;i<maxNumManifolds ;i++)
00319 {
00320 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
00321
00322 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
00323 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
00324
00326 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
00327 ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
00328 {
00329
00330
00331 if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
00332 {
00333 if (colObj0->hasContactResponse())
00334 colObj1->activate();
00335 }
00336 if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
00337 {
00338 if (colObj1->hasContactResponse())
00339 colObj0->activate();
00340 }
00341 if(m_splitIslands)
00342 {
00343
00344 if (dispatcher->needsResponse(colObj0,colObj1))
00345 m_islandmanifold.push_back(manifold);
00346 }
00347 }
00348 }
00349 }
00350
00351
00352
00354 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
00355 {
00356 btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
00357
00358 buildIslands(dispatcher,collisionWorld);
00359
00360 int endIslandIndex=1;
00361 int startIslandIndex;
00362 int numElem = getUnionFind().getNumElements();
00363
00364 BT_PROFILE("processIslands");
00365
00366 if(!m_splitIslands)
00367 {
00368 btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
00369 int maxNumManifolds = dispatcher->getNumManifolds();
00370 callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
00371 }
00372 else
00373 {
00374
00375
00376
00377
00378 int numManifolds = int (m_islandmanifold.size());
00379
00380
00381
00382 m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
00383
00384
00385
00386
00387 int startManifoldIndex = 0;
00388 int endManifoldIndex = 1;
00389
00390
00391
00392
00393
00394
00395
00396
00397 for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
00398 {
00399 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
00400
00401
00402 bool islandSleeping = true;
00403
00404 for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
00405 {
00406 int i = getUnionFind().getElement(endIslandIndex).m_sz;
00407 btCollisionObject* colObj0 = collisionObjects[i];
00408 m_islandBodies.push_back(colObj0);
00409 if (colObj0->isActive())
00410 islandSleeping = false;
00411 }
00412
00413
00414
00415 int numIslandManifolds = 0;
00416 btPersistentManifold** startManifold = 0;
00417
00418 if (startManifoldIndex<numManifolds)
00419 {
00420 int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
00421 if (curIslandId == islandId)
00422 {
00423 startManifold = &m_islandmanifold[startManifoldIndex];
00424
00425 for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
00426 {
00427
00428 }
00430 numIslandManifolds = endManifoldIndex-startManifoldIndex;
00431 }
00432
00433 }
00434
00435 if (!islandSleeping)
00436 {
00437 callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
00438
00439 }
00440
00441 if (numIslandManifolds)
00442 {
00443 startManifoldIndex = endManifoldIndex;
00444 }
00445
00446 m_islandBodies.resize(0);
00447 }
00448 }
00449
00450 }