diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp | 1023 |
1 files changed, 497 insertions, 526 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp index fc54f0ba6e..17287aa82a 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btScalar.h" #include "LinearMath/btThreads.h" #include "btSimulationIslandManagerMt.h" @@ -27,273 +26,259 @@ subject to the following restrictions: //#include <stdio.h> #include "LinearMath/btQuickprof.h" - -SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints ) +SIMD_FORCE_INLINE int calcBatchCost(int bodies, int manifolds, int constraints) { - // rough estimate of the cost of a batch, used for merging - int batchCost = bodies + 8 * manifolds + 4 * constraints; - return batchCost; + // rough estimate of the cost of a batch, used for merging + int batchCost = bodies + 8 * manifolds + 4 * constraints; + return batchCost; } - -SIMD_FORCE_INLINE int calcBatchCost( const btSimulationIslandManagerMt::Island* island ) +SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island) { - return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() ); + return calcBatchCost(island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size()); } - btSimulationIslandManagerMt::btSimulationIslandManagerMt() { - m_minimumSolverBatchSize = calcBatchCost(0, 128, 0); - m_batchIslandMinBodyCount = 32; - m_islandDispatch = parallelIslandDispatch; - m_batchIsland = NULL; + m_minimumSolverBatchSize = calcBatchCost(0, 128, 0); + m_batchIslandMinBodyCount = 32; + m_islandDispatch = parallelIslandDispatch; + m_batchIsland = NULL; } - btSimulationIslandManagerMt::~btSimulationIslandManagerMt() { - for ( int i = 0; i < m_allocatedIslands.size(); ++i ) - { - delete m_allocatedIslands[ i ]; - } - m_allocatedIslands.resize( 0 ); - m_activeIslands.resize( 0 ); - m_freeIslands.resize( 0 ); + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + delete m_allocatedIslands[i]; + } + m_allocatedIslands.resize(0); + m_activeIslands.resize(0); + m_freeIslands.resize(0); } - -inline int getIslandId(const btPersistentManifold* lhs) +inline int getIslandId(const btPersistentManifold* lhs) { const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0()); const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1()); - int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); + int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); return islandId; } - -SIMD_FORCE_INLINE int btGetConstraintIslandId( const btTypedConstraint* lhs ) +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); - return islandId; + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; } /// function object that routes calls to operator< class IslandBatchSizeSortPredicate { public: - bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const - { - int lCost = calcBatchCost( lhs ); - int rCost = calcBatchCost( rhs ); - return lCost > rCost; - } + bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const + { + int lCost = calcBatchCost(lhs); + int rCost = calcBatchCost(rhs); + return lCost > rCost; + } }; - class IslandBodyCapacitySortPredicate { public: - bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const - { - return lhs->bodyArray.capacity() > rhs->bodyArray.capacity(); - } + bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const + { + return lhs->bodyArray.capacity() > rhs->bodyArray.capacity(); + } }; - -void btSimulationIslandManagerMt::Island::append( const Island& other ) +void btSimulationIslandManagerMt::Island::append(const Island& other) { - // append bodies - for ( int i = 0; i < other.bodyArray.size(); ++i ) - { - bodyArray.push_back( other.bodyArray[ i ] ); - } - // append manifolds - for ( int i = 0; i < other.manifoldArray.size(); ++i ) - { - manifoldArray.push_back( other.manifoldArray[ i ] ); - } - // append constraints - for ( int i = 0; i < other.constraintArray.size(); ++i ) - { - constraintArray.push_back( other.constraintArray[ i ] ); - } + // append bodies + for (int i = 0; i < other.bodyArray.size(); ++i) + { + bodyArray.push_back(other.bodyArray[i]); + } + // append manifolds + for (int i = 0; i < other.manifoldArray.size(); ++i) + { + manifoldArray.push_back(other.manifoldArray[i]); + } + // append constraints + for (int i = 0; i < other.constraintArray.size(); ++i) + { + constraintArray.push_back(other.constraintArray[i]); + } } - -bool btIsBodyInIsland( const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj ) +bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj) { - for ( int i = 0; i < island.bodyArray.size(); ++i ) - { - if ( island.bodyArray[ i ] == obj ) - { - return true; - } - } - return false; + for (int i = 0; i < island.bodyArray.size(); ++i) + { + if (island.bodyArray[i] == obj) + { + return true; + } + } + return false; } - void btSimulationIslandManagerMt::initIslandPools() { - // reset island pools - int numElem = getUnionFind().getNumElements(); - m_lookupIslandFromId.resize( numElem ); - for ( int i = 0; i < m_lookupIslandFromId.size(); ++i ) - { - m_lookupIslandFromId[ i ] = NULL; - } - m_activeIslands.resize( 0 ); - m_freeIslands.resize( 0 ); - // check whether allocated islands are sorted by body capacity (largest to smallest) - int lastCapacity = 0; - bool isSorted = true; - for ( int i = 0; i < m_allocatedIslands.size(); ++i ) - { - Island* island = m_allocatedIslands[ i ]; - int cap = island->bodyArray.capacity(); - if ( cap > lastCapacity ) - { - isSorted = false; - break; - } - lastCapacity = cap; - } - if ( !isSorted ) - { - m_allocatedIslands.quickSort( IslandBodyCapacitySortPredicate() ); - } - - m_batchIsland = NULL; - // mark all islands free (but avoid deallocation) - for ( int i = 0; i < m_allocatedIslands.size(); ++i ) - { - Island* island = m_allocatedIslands[ i ]; - island->bodyArray.resize( 0 ); - island->manifoldArray.resize( 0 ); - island->constraintArray.resize( 0 ); - island->id = -1; - island->isSleeping = true; - m_freeIslands.push_back( island ); - } -} + // reset island pools + int numElem = getUnionFind().getNumElements(); + m_lookupIslandFromId.resize(numElem); + for (int i = 0; i < m_lookupIslandFromId.size(); ++i) + { + m_lookupIslandFromId[i] = NULL; + } + m_activeIslands.resize(0); + m_freeIslands.resize(0); + // check whether allocated islands are sorted by body capacity (largest to smallest) + int lastCapacity = 0; + bool isSorted = true; + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + Island* island = m_allocatedIslands[i]; + int cap = island->bodyArray.capacity(); + if (cap > lastCapacity) + { + isSorted = false; + break; + } + lastCapacity = cap; + } + if (!isSorted) + { + m_allocatedIslands.quickSort(IslandBodyCapacitySortPredicate()); + } + m_batchIsland = NULL; + // mark all islands free (but avoid deallocation) + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + Island* island = m_allocatedIslands[i]; + island->bodyArray.resize(0); + island->manifoldArray.resize(0); + island->constraintArray.resize(0); + island->id = -1; + island->isSleeping = true; + m_freeIslands.push_back(island); + } +} -btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland( int id ) +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id) { - Island* island = m_lookupIslandFromId[ id ]; - if ( island == NULL ) - { - // search for existing island - for ( int i = 0; i < m_activeIslands.size(); ++i ) - { - if ( m_activeIslands[ i ]->id == id ) - { - island = m_activeIslands[ i ]; - break; - } - } - m_lookupIslandFromId[ id ] = island; - } - return island; + Island* island = m_lookupIslandFromId[id]; + if (island == NULL) + { + // search for existing island + for (int i = 0; i < m_activeIslands.size(); ++i) + { + if (m_activeIslands[i]->id == id) + { + island = m_activeIslands[i]; + break; + } + } + m_lookupIslandFromId[id] = island; + } + return island; } - -btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland( int id, int numBodies ) +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland(int id, int numBodies) { - Island* island = NULL; - int allocSize = numBodies; - if ( numBodies < m_batchIslandMinBodyCount ) - { - if ( m_batchIsland ) - { - island = m_batchIsland; - m_lookupIslandFromId[ id ] = island; - // if we've made a large enough batch, - if ( island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount ) - { - // next time start a new batch - m_batchIsland = NULL; - } - return island; - } - else - { - // need to allocate a batch island - allocSize = m_batchIslandMinBodyCount * 2; - } - } - btAlignedObjectArray<Island*>& freeIslands = m_freeIslands; - - // search for free island - if ( freeIslands.size() > 0 ) - { - // try to reuse a previously allocated island - int iFound = freeIslands.size(); - // linear search for smallest island that can hold our bodies - for ( int i = freeIslands.size() - 1; i >= 0; --i ) - { - if ( freeIslands[ i ]->bodyArray.capacity() >= allocSize ) - { - iFound = i; - island = freeIslands[ i ]; - island->id = id; - break; - } - } - // if found, shrink array while maintaining ordering - if ( island ) - { - int iDest = iFound; - int iSrc = iDest + 1; - while ( iSrc < freeIslands.size() ) - { - freeIslands[ iDest++ ] = freeIslands[ iSrc++ ]; - } - freeIslands.pop_back(); - } - } - if ( island == NULL ) - { - // no free island found, allocate - island = new Island(); // TODO: change this to use the pool allocator - island->id = id; - island->bodyArray.reserve( allocSize ); - m_allocatedIslands.push_back( island ); - } - m_lookupIslandFromId[ id ] = island; - if ( numBodies < m_batchIslandMinBodyCount ) - { - m_batchIsland = island; - } - m_activeIslands.push_back( island ); - return island; -} + Island* island = NULL; + int allocSize = numBodies; + if (numBodies < m_batchIslandMinBodyCount) + { + if (m_batchIsland) + { + island = m_batchIsland; + m_lookupIslandFromId[id] = island; + // if we've made a large enough batch, + if (island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount) + { + // next time start a new batch + m_batchIsland = NULL; + } + return island; + } + else + { + // need to allocate a batch island + allocSize = m_batchIslandMinBodyCount * 2; + } + } + btAlignedObjectArray<Island*>& freeIslands = m_freeIslands; + // search for free island + if (freeIslands.size() > 0) + { + // try to reuse a previously allocated island + int iFound = freeIslands.size(); + // linear search for smallest island that can hold our bodies + for (int i = freeIslands.size() - 1; i >= 0; --i) + { + if (freeIslands[i]->bodyArray.capacity() >= allocSize) + { + iFound = i; + island = freeIslands[i]; + island->id = id; + break; + } + } + // if found, shrink array while maintaining ordering + if (island) + { + int iDest = iFound; + int iSrc = iDest + 1; + while (iSrc < freeIslands.size()) + { + freeIslands[iDest++] = freeIslands[iSrc++]; + } + freeIslands.pop_back(); + } + } + if (island == NULL) + { + // no free island found, allocate + island = new Island(); // TODO: change this to use the pool allocator + island->id = id; + island->bodyArray.reserve(allocSize); + m_allocatedIslands.push_back(island); + } + m_lookupIslandFromId[id] = island; + if (numBodies < m_batchIslandMinBodyCount) + { + m_batchIsland = island; + } + m_activeIslands.push_back(island); + return island; +} -void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld ) +void btSimulationIslandManagerMt::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld) { - BT_PROFILE("buildIslands"); - + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); //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 endIslandIndex = 1; int startIslandIndex; //update the sleeping state for bodies, if all are sleeping - for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + 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++) + for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) { } @@ -302,21 +287,21 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl bool allSleeping = true; int idx; - for (idx=startIslandIndex;idx<endIslandIndex;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"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if (colObj0->getActivationState()== ACTIVE_TAG || - colObj0->getActivationState()== DISABLE_DEACTIVATION) + if (colObj0->getActivationState() == ACTIVE_TAG || + colObj0->getActivationState() == DISABLE_DEACTIVATION) { allSleeping = false; break; @@ -327,43 +312,43 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl if (allSleeping) { int idx; - for (idx=startIslandIndex;idx<endIslandIndex;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"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - colObj0->setActivationState( ISLAND_SLEEPING ); + colObj0->setActivationState(ISLAND_SLEEPING); } } - } else + } + else { - int idx; - for (idx=startIslandIndex;idx<endIslandIndex;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"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) + if (colObj0->getActivationState() == ISLAND_SLEEPING) { - colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setActivationState(WANTS_DEACTIVATION); colObj0->setDeactivationTime(0.f); } } @@ -372,352 +357,338 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl } } - -void btSimulationIslandManagerMt::addBodiesToIslands( btCollisionWorld* collisionWorld ) +void btSimulationIslandManagerMt::addBodiesToIslands(btCollisionWorld* collisionWorld) { - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - int endIslandIndex = 1; - int startIslandIndex; - int numElem = getUnionFind().getNumElements(); - - // create explicit islands and add bodies to each - for ( startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex ) - { - int islandId = getUnionFind().getElement( startIslandIndex ).m_id; - - // find end index - for ( endIslandIndex = startIslandIndex; ( endIslandIndex < numElem ) && ( getUnionFind().getElement( endIslandIndex ).m_id == islandId ); endIslandIndex++ ) - { - } - // check if island is sleeping - bool islandSleeping = true; - for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ ) - { - int i = getUnionFind().getElement( iElem ).m_sz; - btCollisionObject* colObj = collisionObjects[ i ]; - if ( colObj->isActive() ) - { - islandSleeping = false; - } - } - if ( !islandSleeping ) - { - // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures - int numBodies = endIslandIndex - startIslandIndex; - Island* island = allocateIsland( islandId, numBodies ); - island->isSleeping = false; - - // add bodies to island - for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ ) - { - int i = getUnionFind().getElement( iElem ).m_sz; - btCollisionObject* colObj = collisionObjects[ i ]; - island->bodyArray.push_back( colObj ); - } - } - } + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + int endIslandIndex = 1; + int startIslandIndex; + int numElem = getUnionFind().getNumElements(); -} + // create explicit islands and add bodies to each + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; + // find end index + for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) + { + } + // check if island is sleeping + bool islandSleeping = true; + for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++) + { + int i = getUnionFind().getElement(iElem).m_sz; + btCollisionObject* colObj = collisionObjects[i]; + if (colObj->isActive()) + { + islandSleeping = false; + } + } + if (!islandSleeping) + { + // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures + int numBodies = endIslandIndex - startIslandIndex; + Island* island = allocateIsland(islandId, numBodies); + island->isSleeping = false; -void btSimulationIslandManagerMt::addManifoldsToIslands( btDispatcher* dispatcher ) -{ - // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island - int maxNumManifolds = dispatcher->getNumManifolds(); - for ( int 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() ); - - ///@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 ) - { - if ( colObj0->hasContactResponse() ) - colObj1->activate(); - } - if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING ) - { - if ( colObj1->hasContactResponse() ) - colObj0->activate(); - } - //filtering for response - if ( dispatcher->needsResponse( colObj0, colObj1 ) ) - { - // scatter manifolds into various islands - int islandId = getIslandId( manifold ); - // if island not sleeping, - if ( Island* island = getIsland( islandId ) ) - { - island->manifoldArray.push_back( manifold ); - } - } - } - } + // add bodies to island + for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++) + { + int i = getUnionFind().getElement(iElem).m_sz; + btCollisionObject* colObj = collisionObjects[i]; + island->bodyArray.push_back(colObj); + } + } + } } - -void btSimulationIslandManagerMt::addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints ) +void btSimulationIslandManagerMt::addManifoldsToIslands(btDispatcher* dispatcher) { - // walk constraints - for ( int i = 0; i < constraints.size(); i++ ) - { - // scatter constraints into various islands - btTypedConstraint* constraint = constraints[ i ]; - if ( constraint->isEnabled() ) - { - int islandId = btGetConstraintIslandId( constraint ); - // if island is not sleeping, - if ( Island* island = getIsland( islandId ) ) - { - island->constraintArray.push_back( constraint ); - } - } - } + // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island + int maxNumManifolds = dispatcher->getNumManifolds(); + for (int 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()); + + ///@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) + { + if (colObj0->hasContactResponse()) + colObj1->activate(); + } + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + { + if (colObj1->hasContactResponse()) + colObj0->activate(); + } + //filtering for response + if (dispatcher->needsResponse(colObj0, colObj1)) + { + // scatter manifolds into various islands + int islandId = getIslandId(manifold); + // if island not sleeping, + if (Island* island = getIsland(islandId)) + { + island->manifoldArray.push_back(manifold); + } + } + } + } } +void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints) +{ + // walk constraints + for (int i = 0; i < constraints.size(); i++) + { + // scatter constraints into various islands + btTypedConstraint* constraint = constraints[i]; + if (constraint->isEnabled()) + { + int islandId = btGetConstraintIslandId(constraint); + // if island is not sleeping, + if (Island* island = getIsland(islandId)) + { + island->constraintArray.push_back(constraint); + } + } + } +} void btSimulationIslandManagerMt::mergeIslands() { - // sort islands in order of decreasing batch size - m_activeIslands.quickSort( IslandBatchSizeSortPredicate() ); - - // merge small islands to satisfy minimum batch size - // find first small batch island - int destIslandIndex = m_activeIslands.size(); - for ( int i = 0; i < m_activeIslands.size(); ++i ) - { - Island* island = m_activeIslands[ i ]; - int batchSize = calcBatchCost( island ); - if ( batchSize < m_minimumSolverBatchSize ) - { - destIslandIndex = i; - break; - } - } - int lastIndex = m_activeIslands.size() - 1; - while ( destIslandIndex < lastIndex ) - { - // merge islands from the back of the list - Island* island = m_activeIslands[ destIslandIndex ]; - int numBodies = island->bodyArray.size(); - int numManifolds = island->manifoldArray.size(); - int numConstraints = island->constraintArray.size(); - int firstIndex = lastIndex; - // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have - while ( true ) - { - Island* src = m_activeIslands[ firstIndex ]; - numBodies += src->bodyArray.size(); - numManifolds += src->manifoldArray.size(); - numConstraints += src->constraintArray.size(); - int batchCost = calcBatchCost( numBodies, numManifolds, numConstraints ); - if ( batchCost >= m_minimumSolverBatchSize ) - { - break; - } - if ( firstIndex - 1 == destIslandIndex ) - { - break; - } - firstIndex--; - } - // reserve space for these pointers to minimize reallocation - island->bodyArray.reserve( numBodies ); - island->manifoldArray.reserve( numManifolds ); - island->constraintArray.reserve( numConstraints ); - // merge islands - for ( int i = firstIndex; i <= lastIndex; ++i ) - { - island->append( *m_activeIslands[ i ] ); - } - // shrink array to exclude the islands that were merged from - m_activeIslands.resize( firstIndex ); - lastIndex = firstIndex - 1; - destIslandIndex++; - } -} + // sort islands in order of decreasing batch size + m_activeIslands.quickSort(IslandBatchSizeSortPredicate()); + // merge small islands to satisfy minimum batch size + // find first small batch island + int destIslandIndex = m_activeIslands.size(); + for (int i = 0; i < m_activeIslands.size(); ++i) + { + Island* island = m_activeIslands[i]; + int batchSize = calcBatchCost(island); + if (batchSize < m_minimumSolverBatchSize) + { + destIslandIndex = i; + break; + } + } + int lastIndex = m_activeIslands.size() - 1; + while (destIslandIndex < lastIndex) + { + // merge islands from the back of the list + Island* island = m_activeIslands[destIslandIndex]; + int numBodies = island->bodyArray.size(); + int numManifolds = island->manifoldArray.size(); + int numConstraints = island->constraintArray.size(); + int firstIndex = lastIndex; + // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have + while (true) + { + Island* src = m_activeIslands[firstIndex]; + numBodies += src->bodyArray.size(); + numManifolds += src->manifoldArray.size(); + numConstraints += src->constraintArray.size(); + int batchCost = calcBatchCost(numBodies, numManifolds, numConstraints); + if (batchCost >= m_minimumSolverBatchSize) + { + break; + } + if (firstIndex - 1 == destIslandIndex) + { + break; + } + firstIndex--; + } + // reserve space for these pointers to minimize reallocation + island->bodyArray.reserve(numBodies); + island->manifoldArray.reserve(numManifolds); + island->constraintArray.reserve(numConstraints); + // merge islands + for (int i = firstIndex; i <= lastIndex; ++i) + { + island->append(*m_activeIslands[i]); + } + // shrink array to exclude the islands that were merged from + m_activeIslands.resize(firstIndex); + lastIndex = firstIndex - 1; + destIslandIndex++; + } +} void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams) { - btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[ 0 ] : NULL; - btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[ 0 ] : NULL; - solver->solveGroup( &island.bodyArray[ 0 ], - island.bodyArray.size(), - manifolds, - island.manifoldArray.size(), - constraintsPtr, - island.constraintArray.size(), - *solverParams.m_solverInfo, - solverParams.m_debugDrawer, - solverParams.m_dispatcher - ); + btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[0] : NULL; + btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[0] : NULL; + solver->solveGroup(&island.bodyArray[0], + island.bodyArray.size(), + manifolds, + island.manifoldArray.size(), + constraintsPtr, + island.constraintArray.size(), + *solverParams.m_solverInfo, + solverParams.m_debugDrawer, + solverParams.m_dispatcher); } - -void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams ) +void btSimulationIslandManagerMt::serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams) { - BT_PROFILE( "serialIslandDispatch" ); - // serial dispatch - btAlignedObjectArray<Island*>& islands = *islandsPtr; - btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; - for ( int i = 0; i < islands.size(); ++i ) - { - solveIsland(solver, *islands[ i ], solverParams); - } + BT_PROFILE("serialIslandDispatch"); + // serial dispatch + btAlignedObjectArray<Island*>& islands = *islandsPtr; + btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; + for (int i = 0; i < islands.size(); ++i) + { + solveIsland(solver, *islands[i], solverParams); + } } - struct UpdateIslandDispatcher : public btIParallelForBody { - btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr; - const btSimulationIslandManagerMt::SolverParams& m_solverParams; - - UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams) - : m_islandsPtr(islandsPtr), m_solverParams(solverParams) - {} - - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - btConstraintSolver* solver = m_solverParams.m_solverPool; - for ( int i = iBegin; i < iEnd; ++i ) - { - btSimulationIslandManagerMt::Island* island = m_islandsPtr[ i ]; - btSimulationIslandManagerMt::solveIsland( solver, *island, m_solverParams ); - } - } -}; + btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr; + const btSimulationIslandManagerMt::SolverParams& m_solverParams; + UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams) + : m_islandsPtr(islandsPtr), m_solverParams(solverParams) + { + } + + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + btConstraintSolver* solver = m_solverParams.m_solverPool; + for (int i = iBegin; i < iEnd; ++i) + { + btSimulationIslandManagerMt::Island* island = m_islandsPtr[i]; + btSimulationIslandManagerMt::solveIsland(solver, *island, m_solverParams); + } + } +}; -void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams ) +void btSimulationIslandManagerMt::parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams) { - BT_PROFILE( "parallelIslandDispatch" ); - // - // if there are islands with many contacts, it may be faster to submit these - // large islands *serially* to a single parallel constraint solver, and then later - // submit the remaining smaller islands in parallel to multiple sequential solvers. - // - // Some task schedulers do not deal well with nested parallelFor loops. One implementation - // of OpenMP was actually slower than doing everything single-threaded. Intel TBB - // on the other hand, seems to do a pretty respectable job with it. - // - // When solving islands in parallel, the worst case performance happens when there - // is one very large island and then perhaps a smattering of very small - // islands -- one worker thread takes the large island and the remaining workers - // tear through the smaller islands and then sit idle waiting for the first worker - // to finish. Solving islands in parallel works best when there are numerous small - // islands, roughly equal in size. - // - // By contrast, the other approach -- the parallel constraint solver -- is only - // able to deliver a worthwhile speedup when the island is large. For smaller islands, - // it is difficult to extract a useful amount of parallelism -- the overhead of grouping - // the constraints into batches and sending the batches to worker threads can nullify - // any gains from parallelism. - // - - UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams); - // We take advantage of the fact the islands are sorted in order of decreasing size - int iBegin = 0; - if (solverParams.m_solverMt) - { - while ( iBegin < islandsPtr->size() ) - { - btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ iBegin ]; - if ( island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching ) - { - // OK to submit the rest of the array in parallel - break; - } - // serial dispatch to parallel solver for large islands (if any) - solveIsland(solverParams.m_solverMt, *island, solverParams); - ++iBegin; - } - } - // parallel dispatch to sequential solvers for rest - btParallelFor( iBegin, islandsPtr->size(), 1, dispatcher ); + BT_PROFILE("parallelIslandDispatch"); + // + // if there are islands with many contacts, it may be faster to submit these + // large islands *serially* to a single parallel constraint solver, and then later + // submit the remaining smaller islands in parallel to multiple sequential solvers. + // + // Some task schedulers do not deal well with nested parallelFor loops. One implementation + // of OpenMP was actually slower than doing everything single-threaded. Intel TBB + // on the other hand, seems to do a pretty respectable job with it. + // + // When solving islands in parallel, the worst case performance happens when there + // is one very large island and then perhaps a smattering of very small + // islands -- one worker thread takes the large island and the remaining workers + // tear through the smaller islands and then sit idle waiting for the first worker + // to finish. Solving islands in parallel works best when there are numerous small + // islands, roughly equal in size. + // + // By contrast, the other approach -- the parallel constraint solver -- is only + // able to deliver a worthwhile speedup when the island is large. For smaller islands, + // it is difficult to extract a useful amount of parallelism -- the overhead of grouping + // the constraints into batches and sending the batches to worker threads can nullify + // any gains from parallelism. + // + + UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams); + // We take advantage of the fact the islands are sorted in order of decreasing size + int iBegin = 0; + if (solverParams.m_solverMt) + { + while (iBegin < islandsPtr->size()) + { + btSimulationIslandManagerMt::Island* island = (*islandsPtr)[iBegin]; + if (island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching) + { + // OK to submit the rest of the array in parallel + break; + } + // serial dispatch to parallel solver for large islands (if any) + solveIsland(solverParams.m_solverMt, *island, solverParams); + ++iBegin; + } + } + // parallel dispatch to sequential solvers for rest + btParallelFor(iBegin, islandsPtr->size(), 1, dispatcher); } - ///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher, - btCollisionWorld* collisionWorld, - btAlignedObjectArray<btTypedConstraint*>& constraints, - const SolverParams& solverParams - ) +void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray<btTypedConstraint*>& constraints, + const SolverParams& solverParams) { BT_PROFILE("buildAndProcessIslands"); btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - buildIslands(dispatcher,collisionWorld); + buildIslands(dispatcher, collisionWorld); - if(!getSplitIslands()) + if (!getSplitIslands()) { - btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); - int maxNumManifolds = dispatcher->getNumManifolds(); - - for ( int i = 0; i < maxNumManifolds; i++ ) - { - btPersistentManifold* manifold = manifolds[ i ]; - - const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() ); - const btCollisionObject* colObj1 = static_cast<const 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 ) - { - if ( colObj0->hasContactResponse() ) - colObj1->activate(); - } - if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING ) - { - if ( colObj1->hasContactResponse() ) - colObj0->activate(); - } - } - } - btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL; - btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; - solver->solveGroup(&collisionObjects[0], - collisionObjects.size(), - manifolds, - maxNumManifolds, - constraintsPtr, - constraints.size(), - *solverParams.m_solverInfo, - solverParams.m_debugDrawer, - solverParams.m_dispatcher - ); + btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); + + for (int i = 0; i < maxNumManifolds; i++) + { + btPersistentManifold* manifold = manifolds[i]; + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const 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) + { + if (colObj0->hasContactResponse()) + colObj1->activate(); + } + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + { + if (colObj1->hasContactResponse()) + colObj0->activate(); + } + } + } + btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[0] : NULL; + btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; + solver->solveGroup(&collisionObjects[0], + collisionObjects.size(), + manifolds, + maxNumManifolds, + constraintsPtr, + constraints.size(), + *solverParams.m_solverInfo, + solverParams.m_debugDrawer, + solverParams.m_dispatcher); } else { - initIslandPools(); - - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - addBodiesToIslands( collisionWorld ); - addManifoldsToIslands( dispatcher ); - addConstraintsToIslands( constraints ); - - // m_activeIslands array should now contain all non-sleeping Islands, and each Island should - // have all the necessary bodies, manifolds and constraints. - - // if we want to merge islands with small batch counts, - if ( m_minimumSolverBatchSize > 1 ) - { - mergeIslands(); - } - // dispatch islands to solver - m_islandDispatch( &m_activeIslands, solverParams ); + initIslandPools(); + + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + addBodiesToIslands(collisionWorld); + addManifoldsToIslands(dispatcher); + addConstraintsToIslands(constraints); + + // m_activeIslands array should now contain all non-sleeping Islands, and each Island should + // have all the necessary bodies, manifolds and constraints. + + // if we want to merge islands with small batch counts, + if (m_minimumSolverBatchSize > 1) + { + mergeIslands(); + } + // dispatch islands to solver + m_islandDispatch(&m_activeIslands, solverParams); } } |