| /* | |
| Bullet Continuous Collision Detection and Physics Library | |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 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. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| */ | |
| #include "LinearMath/btScalar.h" | |
| #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" | |
| #include "BulletCollision/BroadphaseCollision/btDispatcher.h" | |
| #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" | |
| #include "BulletCollision/CollisionDispatch/btCollisionObject.h" | |
| #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" | |
| //#include <stdio.h> | |
| #include "LinearMath/btQuickprof.h" | |
| btSimulationIslandManager::btSimulationIslandManager(): | |
| m_splitIslands(true) | |
| { | |
| } | |
| btSimulationIslandManager::~btSimulationIslandManager() | |
| { | |
| } | |
| void btSimulationIslandManager::initUnionFind(int n) | |
| { | |
| m_unionFind.reset(n); | |
| } | |
| void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) | |
| { | |
| { | |
| for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++) | |
| { | |
| btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); | |
| 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()); | |
| } | |
| } | |
| } | |
| } | |
| void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) | |
| { | |
| initUnionFind( int (colWorld->getCollisionObjectArray().size())); | |
| // put the index into m_controllers into m_tag | |
| { | |
| 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++; | |
| } | |
| } | |
| // do the union find | |
| findUnions(dispatcher,colWorld); | |
| } | |
| void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) | |
| { | |
| // put the islandId ('find' value) into m_tag | |
| { | |
| 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++; | |
| } | |
| } | |
| } | |
| 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; | |
| } | |
| /// function object that routes calls to operator< | |
| class btPersistentManifoldSortPredicate | |
| { | |
| public: | |
| SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) | |
| { | |
| return getIslandId(lhs) < getIslandId(rhs); | |
| } | |
| }; | |
| void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) | |
| { | |
| BT_PROFILE("islandUnionFindAndQuickSort"); | |
| btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); | |
| m_islandmanifold.resize(0); | |
| //we are going to sort the unionfind array, and store the element id in the size | |
| //afterwards, we clean unionfind, to make sure no-one uses it anymore | |
| getUnionFind().sortIslands(); | |
| int numElem = getUnionFind().getNumElements(); | |
| int endIslandIndex=1; | |
| int startIslandIndex; | |
| //update the sleeping state for bodies, if all are sleeping | |
| 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++) | |
| { | |
| } | |
| //int numSleeping = 0; | |
| 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)) | |
| { | |
| // printf("error in island management\n"); | |
| } | |
| 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)) | |
| { | |
| // printf("error in island management\n"); | |
| } | |
| 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)) | |
| { | |
| // printf("error in island management\n"); | |
| } | |
| 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(); | |
| //#define SPLIT_ISLANDS 1 | |
| //#ifdef SPLIT_ISLANDS | |
| //#endif //SPLIT_ISLANDS | |
| for (i=0;i<maxNumManifolds ;i++) | |
| { | |
| btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); | |
| btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0()); | |
| btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1()); | |
| ///@todo: check sleeping conditions! | |
| if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || | |
| ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) | |
| { | |
| //kinematic objects don't merge islands, but wake up all connected objects | |
| if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) | |
| { | |
| colObj1->activate(); | |
| } | |
| if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) | |
| { | |
| colObj0->activate(); | |
| } | |
| if(m_splitIslands) | |
| { | |
| //filtering for response | |
| if (dispatcher->needsResponse(colObj0,colObj1)) | |
| m_islandmanifold.push_back(manifold); | |
| } | |
| } | |
| } | |
| } | |
| ///@todo: this is random access, it can be walked 'cache friendly'! | |
| 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 | |
| { | |
| // Sort manifolds, based on islands | |
| // Sort the vector using predicate and std::sort | |
| //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); | |
| int numManifolds = int (m_islandmanifold.size()); | |
| //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) | |
| m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); | |
| //now process all active islands (sets of manifolds for now) | |
| int startManifoldIndex = 0; | |
| int endManifoldIndex = 1; | |
| //int islandId; | |
| // printf("Start Islands\n"); | |
| //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated | |
| for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) | |
| { | |
| int islandId = getUnionFind().getElement(startIslandIndex).m_id; | |
| bool islandSleeping = false; | |
| 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 = true; | |
| } | |
| //find the accompanying contact manifold for this islandId | |
| 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++) | |
| { | |
| } | |
| /// Process the actual simulation, only if not sleeping/deactivated | |
| numIslandManifolds = endManifoldIndex-startManifoldIndex; | |
| } | |
| } | |
| if (!islandSleeping) | |
| { | |
| callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); | |
| // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); | |
| } | |
| if (numIslandManifolds) | |
| { | |
| startManifoldIndex = endManifoldIndex; | |
| } | |
| m_islandBodies.resize(0); | |
| } | |
| } // else if(!splitIslands) | |
| } |