123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450 |
- #include "bullet/LinearMath/btScalar.h"
- #include "btSimulationIslandManager.h"
- #include "bullet/BulletCollision//BroadphaseCollision/btDispatcher.h"
- #include "bullet/BulletCollision//NarrowPhaseCollision/btPersistentManifold.h"
- #include "bullet/BulletCollision//CollisionDispatch/btCollisionObject.h"
- #include "bullet/BulletCollision//CollisionDispatch/btCollisionWorld.h"
- #include "bullet/LinearMath/btQuickprof.h"
- btSimulationIslandManager::btSimulationIslandManager():
- m_splitIslands(true)
- {
- }
- btSimulationIslandManager::~btSimulationIslandManager()
- {
- }
- void btSimulationIslandManager::initUnionFind(int n)
- {
- m_unionFind.reset(n);
- }
-
- void btSimulationIslandManager::findUnions(btDispatcher* ,btCollisionWorld* colWorld)
- {
-
- {
- btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
- const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
- if (numOverlappingPairs)
- {
- btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
-
- for (int i=0;i<numOverlappingPairs;i++)
- {
- const btBroadphasePair& collisionPair = pairPtr[i];
- btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
- btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
- if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
- ((colObj1) && ((colObj1)->mergesSimulationIslands())))
- {
- m_unionFind.unite((colObj0)->getIslandTag(),
- (colObj1)->getIslandTag());
- }
- }
- }
- }
- }
- #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
- void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
- {
-
- int index = 0;
- {
- int i;
- for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
- {
- btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
-
- if (!collisionObject->isStaticOrKinematicObject())
- {
- collisionObject->setIslandTag(index++);
- }
- collisionObject->setCompanionId(-1);
- collisionObject->setHitFraction(btScalar(1.));
- }
- }
-
- initUnionFind( index );
- findUnions(dispatcher,colWorld);
- }
- void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
- {
-
- {
- int index = 0;
- int i;
- for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
- {
- btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
- if (!collisionObject->isStaticOrKinematicObject())
- {
- collisionObject->setIslandTag( m_unionFind.find(index) );
-
- m_unionFind.getElement(index).m_sz = i;
- collisionObject->setCompanionId(-1);
- index++;
- } else
- {
- collisionObject->setIslandTag(-1);
- collisionObject->setCompanionId(-2);
- }
- }
- }
- }
- #else
- void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
- {
- initUnionFind( int (colWorld->getCollisionObjectArray().size()));
-
- {
- int index = 0;
- int i;
- for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
- {
- btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
- collisionObject->setIslandTag(index);
- collisionObject->setCompanionId(-1);
- collisionObject->setHitFraction(btScalar(1.));
- index++;
- }
- }
-
- findUnions(dispatcher,colWorld);
- }
- void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
- {
-
- {
- int index = 0;
- int i;
- for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
- {
- btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
- if (!collisionObject->isStaticOrKinematicObject())
- {
- collisionObject->setIslandTag( m_unionFind.find(index) );
- collisionObject->setCompanionId(-1);
- } else
- {
- collisionObject->setIslandTag(-1);
- collisionObject->setCompanionId(-2);
- }
- index++;
- }
- }
- }
- #endif
- inline int getIslandId(const btPersistentManifold* lhs)
- {
- int islandId;
- const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
- const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
- islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
- return islandId;
- }
- class btPersistentManifoldSortPredicate
- {
- public:
- SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const
- {
- return getIslandId(lhs) < getIslandId(rhs);
- }
- };
- void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
- {
- BT_PROFILE("islandUnionFindAndQuickSort");
-
- btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
- m_islandmanifold.resize(0);
-
-
-
- getUnionFind().sortIslands();
- int numElem = getUnionFind().getNumElements();
- int endIslandIndex=1;
- int startIslandIndex;
-
- for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
- {
- int islandId = getUnionFind().getElement(startIslandIndex).m_id;
- for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
- {
- }
-
- bool allSleeping = true;
- int idx;
- for (idx=startIslandIndex;idx<endIslandIndex;idx++)
- {
- int i = getUnionFind().getElement(idx).m_sz;
- btCollisionObject* colObj0 = collisionObjects[i];
- if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
- {
- }
- btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
- if (colObj0->getIslandTag() == islandId)
- {
- if (colObj0->getActivationState()== ACTIVE_TAG)
- {
- allSleeping = false;
- }
- if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
- {
- allSleeping = false;
- }
- }
- }
-
- if (allSleeping)
- {
- int idx;
- for (idx=startIslandIndex;idx<endIslandIndex;idx++)
- {
- int i = getUnionFind().getElement(idx).m_sz;
- btCollisionObject* colObj0 = collisionObjects[i];
- if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
- {
- }
- btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
- if (colObj0->getIslandTag() == islandId)
- {
- colObj0->setActivationState( ISLAND_SLEEPING );
- }
- }
- } else
- {
- int idx;
- for (idx=startIslandIndex;idx<endIslandIndex;idx++)
- {
- int i = getUnionFind().getElement(idx).m_sz;
- btCollisionObject* colObj0 = collisionObjects[i];
- if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
- {
- }
- btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
- if (colObj0->getIslandTag() == islandId)
- {
- if ( colObj0->getActivationState() == ISLAND_SLEEPING)
- {
- colObj0->setActivationState( WANTS_DEACTIVATION);
- colObj0->setDeactivationTime(0.f);
- }
- }
- }
- }
- }
-
- int i;
- int maxNumManifolds = dispatcher->getNumManifolds();
-
-
- for (i=0;i<maxNumManifolds ;i++)
- {
- btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
-
- const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
- const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
-
-
- if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
- ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
- {
-
-
- if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
- {
- if (colObj0->hasContactResponse())
- colObj1->activate();
- }
- if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
- {
- if (colObj1->hasContactResponse())
- colObj0->activate();
- }
- if(m_splitIslands)
- {
-
- if (dispatcher->needsResponse(colObj0,colObj1))
- m_islandmanifold.push_back(manifold);
- }
- }
- }
- }
- void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
- {
- btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
- buildIslands(dispatcher,collisionWorld);
- int endIslandIndex=1;
- int startIslandIndex;
- int numElem = getUnionFind().getNumElements();
- BT_PROFILE("processIslands");
- if(!m_splitIslands)
- {
- btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
- int maxNumManifolds = dispatcher->getNumManifolds();
- callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
- }
- else
- {
-
-
-
- int numManifolds = int (m_islandmanifold.size());
-
-
- m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
-
-
- int startManifoldIndex = 0;
- int endManifoldIndex = 1;
-
-
-
-
- for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
- {
- int islandId = getUnionFind().getElement(startIslandIndex).m_id;
- bool islandSleeping = true;
-
- for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
- {
- int i = getUnionFind().getElement(endIslandIndex).m_sz;
- btCollisionObject* colObj0 = collisionObjects[i];
- m_islandBodies.push_back(colObj0);
- if (colObj0->isActive())
- islandSleeping = false;
- }
-
-
- int numIslandManifolds = 0;
- btPersistentManifold** startManifold = 0;
- if (startManifoldIndex<numManifolds)
- {
- int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
- if (curIslandId == islandId)
- {
- startManifold = &m_islandmanifold[startManifoldIndex];
-
- for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
- {
- }
-
- numIslandManifolds = endManifoldIndex-startManifoldIndex;
- }
- }
- if (!islandSleeping)
- {
- callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
-
- }
-
- if (numIslandManifolds)
- {
- startManifoldIndex = endManifoldIndex;
- }
- m_islandBodies.resize(0);
- }
- }
- }
|