diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp | 696 |
1 files changed, 696 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp new file mode 100644 index 00000000000..772b7742023 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp @@ -0,0 +1,696 @@ +/* +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 "LinearMath/btThreads.h" +#include "btSimulationIslandManagerMt.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h" // for s_minimumContactManifoldsForBatching + +//#include <stdio.h> +#include "LinearMath/btQuickprof.h" + +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; +} + +SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island) +{ + 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; +} + +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); +} + +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(); + return islandId; +} + +SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs) +{ + 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; + } +}; + +class IslandBodyCapacitySortPredicate +{ +public: + 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) +{ + // 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) +{ + 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); + } +} + +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id) +{ + btAssert(id >= 0); + btAssert(id < m_lookupIslandFromId.size()); + 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) +{ + 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) +{ + 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 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 || + colObj0->getActivationState() == DISABLE_DEACTIVATION) + { + allSleeping = false; + break; + } + } + } + + 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); + } + } + } + } + } +} + +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); + } + } + } +} + +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); + } + } + } + } +} + +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 = btGetConstraintIslandId1(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++; + } +} + +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); +} + +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); + } +} + +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); + } + } +}; + +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); +} + +///@todo: this is random access, it can be walked 'cache friendly'! +void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray<btTypedConstraint*>& constraints, + const SolverParams& solverParams) +{ + BT_PROFILE("buildAndProcessIslands"); + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + buildIslands(dispatcher, collisionWorld); + + 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); + } + 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); + } +} |