diff --git a/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp b/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp index cc1be22a1..521fd8187 100644 --- a/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp +++ b/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//#define USE_PARALLEL_SOLVER 1 //experimental parallel solver +#define USE_PARALLEL_SOLVER 1 //experimental parallel solver #define USE_PARALLEL_DISPATCHER 1 #include "btBulletDynamicsCommon.h" @@ -290,7 +290,9 @@ void MultiThreadedDemo::initPhysics() m_azi = 90.f; m_dispatcher=0; - m_collisionConfiguration = new btDefaultCollisionConfiguration(); + btDefaultCollisionConstructionInfo cci; + cci.m_defaultMaxPersistentManifoldPoolSize = 32768; + m_collisionConfiguration = new btDefaultCollisionConfiguration(cci); #ifdef USE_PARALLEL_DISPATCHER int maxNumOutstandingTasks = 4; @@ -352,6 +354,8 @@ m_threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32Threa btVector3 worldAabbMin(-1000,-1000,-1000); btVector3 worldAabbMax(1000,1000,1000); + + m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies); @@ -359,6 +363,8 @@ m_threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32Threa #ifdef USE_PARALLEL_SOLVER m_threadSupportSolver = createSolverThreadSupport(maxNumOutstandingTasks); m_solver = new btParallelConstraintSolver(m_threadSupportSolver); + //this solver requires the contacts to be in a contiguous pool, so avoid dynamic allocation + m_dispatcher->setDispatcherFlags(btCollisionDispatcher::CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION); #else btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver(); diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 20bcfdf95..83d48ad9e 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -26,6 +26,7 @@ class btOverlappingPairCache; class btPersistentManifold; class btStackAlloc; +class btPoolAllocator; struct btDispatcherInfo { @@ -97,6 +98,10 @@ public: virtual btPersistentManifold** getInternalManifoldPointer() = 0; + virtual btPoolAllocator* getInternalManifoldPool() = 0; + + virtual const btPoolAllocator* getInternalManifoldPool() const = 0; + virtual void* allocateCollisionAlgorithm(int size) = 0; virtual void freeCollisionAlgorithm(void* ptr) = 0; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 691bad35f..29674f3be 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -92,8 +92,16 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); } else { - mem = btAlignedAlloc(sizeof(btPersistentManifold),16); - + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0) + { + mem = btAlignedAlloc(sizeof(btPersistentManifold),16); + } else + { + btAssert(0); + //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration + return 0; + } } btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 651794158..2fca43f82 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -67,7 +67,8 @@ public: enum DispatcherFlags { CD_STATIC_STATIC_REPORTED = 1, - CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2 + CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2, + CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4 }; int getDispatcherFlags() const @@ -155,6 +156,16 @@ public: m_collisionConfiguration = config; } + virtual btPoolAllocator* getInternalManifoldPool() + { + return m_persistentManifoldPoolAllocator; + } + + virtual const btPoolAllocator* getInternalManifoldPool() const + { + return m_persistentManifoldPoolAllocator; + } + }; #endif //BT_COLLISION__DISPATCHER_H diff --git a/src/BulletMultiThreaded/btParallelConstraintSolver.cpp b/src/BulletMultiThreaded/btParallelConstraintSolver.cpp index 7ab3245fb..77e7d3105 100644 --- a/src/BulletMultiThreaded/btParallelConstraintSolver.cpp +++ b/src/BulletMultiThreaded/btParallelConstraintSolver.cpp @@ -17,6 +17,9 @@ subject to the following restrictions: #include "btParallelConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "LinearMath/btPoolAllocator.h" + #include "LinearMath/btQuickprof.h" #include "BulletMultiThreaded/btThreadSupportInterface.h" @@ -153,9 +156,10 @@ void btSolveContactConstraint( void CustomSolveConstraintsTaskParallel( const PfxParallelGroup *contactParallelGroup,const PfxParallelBatch *contactParallelBatches, PfxConstraintPair *contactPairs,uint32_t numContactPairs, - btPersistentManifold* offsetContactManifolds__, + btPersistentManifold* offsetContactManifolds, const PfxParallelGroup *jointParallelGroup,const PfxParallelBatch *jointParallelBatches, PfxConstraintPair *jointPairs,uint32_t numJointPairs, + btSolverConstraint* offsetSolverConstraints, TrbState *offsetRigStates, PfxSolverBody *offsetSolverBodies, uint32_t numRigidBodies, @@ -188,7 +192,7 @@ void CustomSolveConstraintsTaskParallel( } else { - btSolverConstraint* constraintRow = (btSolverConstraint*) pfxGetContactId1(pair); + btSolverConstraint* constraintRow = &offsetSolverConstraints[pfxGetContactId1(pair)]; int numRows = pfxGetNumConstraints(pair); int i; for (i=0;isolveConstraints.jointParallelBatches, io->solveConstraints.jointPairs, io->solveConstraints.numJointPairs, - - io->solveConstraints.offsetRigStates, + io->solveConstraints.offsetSolverConstraints, + io->solveConstraints.offsetRigStates1, io->solveConstraints.offsetSolverBodies, io->solveConstraints.numRigidBodies, io->solveConstraints.iteration, @@ -518,7 +524,7 @@ void SolverThreadFunc(void* userPtr,void* lsMemory) if(batch > 0) { CustomSetupContactConstraintsTask( io->setupContactConstraints.offsetContactPairs+start,batch, -// io->setupContactConstraints.offsetContactManifolds, + io->setupContactConstraints.offsetContactManifolds, io->setupContactConstraints.offsetRigStates, // io->setupContactConstraints.offsetRigBodies, io->setupContactConstraints.offsetSolverBodies, @@ -579,6 +585,7 @@ void CustomSetupContactConstraintsNew( io[t].setupContactConstraints.offsetContactPairs = contactPairs1; io[t].setupContactConstraints.numContactPairs1 = numContactPairs; io[t].setupContactConstraints.offsetRigStates = offsetRigStates; + io[t].setupContactConstraints.offsetContactManifolds = offsetContactManifolds; io[t].setupContactConstraints.offsetSolverBodies = offsetSolverBodies; io[t].setupContactConstraints.numRigidBodies = numRigidBodies; io[t].setupContactConstraints.separateBias = separationBias; @@ -593,7 +600,7 @@ void CustomSetupContactConstraintsNew( //#define SEQUENTIAL_SETUP #ifdef SEQUENTIAL_SETUP - CustomSetupContactConstraintsTask(contactPairs1,numContactPairs,offsetRigStates,offsetSolverBodies,numRigidBodies,separationBias,timeStep); + CustomSetupContactConstraintsTask(contactPairs1,numContactPairs,offsetContactManifolds,offsetRigStates,offsetSolverBodies,numRigidBodies,separationBias,timeStep); #else threadSupport->sendRequest(1,(ppu_address_t)&io[t],t); #endif @@ -722,7 +729,8 @@ void CustomSolveConstraintsParallel( PfxConstraintPair *contactPairs,uint32_t numContactPairs, PfxConstraintPair *jointPairs,uint32_t numJointPairs, - + btPersistentManifold* offsetContactManifolds, + btSolverConstraint* offsetSolverConstraints, TrbState *offsetRigStates, PfxSolverBody *offsetSolverBodies, uint32_t numRigidBodies, @@ -782,12 +790,13 @@ void CustomSolveConstraintsParallel( io[t].solveConstraints.contactParallelBatches = cbatches; io[t].solveConstraints.contactPairs = contactPairs; io[t].solveConstraints.numContactPairs = numContactPairs; -// io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds; + io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds; io[t].solveConstraints.jointParallelGroup = jgroup; io[t].solveConstraints.jointParallelBatches = jbatches; io[t].solveConstraints.jointPairs = jointPairs; io[t].solveConstraints.numJointPairs = numJointPairs; - io[t].solveConstraints.offsetRigStates = offsetRigStates; + io[t].solveConstraints.offsetSolverConstraints = offsetSolverConstraints; + io[t].solveConstraints.offsetRigStates1 = offsetRigStates; io[t].solveConstraints.offsetSolverBodies = offsetSolverBodies; io[t].solveConstraints.numRigidBodies = numRigidBodies; io[t].solveConstraints.iteration = iteration; @@ -856,10 +865,11 @@ void CustomSolveConstraintsParallel( void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphasePair *new_pairs1 , + btPersistentManifold* offsetContactManifolds, TrbState* states,int numRigidBodies, struct PfxSolverBody* solverBodies, - btPersistentManifold* contacts, PfxConstraintPair* jointPairs, unsigned int numJoints, + btSolverConstraint* offsetSolverConstraints, float separateBias, float timeStep, int iteration, @@ -908,7 +918,8 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase timeStep); #else CustomSetupContactConstraintsNew( - (PfxConstraintPair*)new_pairs1,new_num,contacts, + (PfxConstraintPair*)new_pairs1,new_num, + offsetContactManifolds, states, solverBodies, numRigidBodies, @@ -939,6 +950,8 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase CustomSolveConstraintsParallel( (PfxConstraintPair*)new_pairs1,new_num, jointPairs,numJoints, + offsetContactManifolds, + offsetSolverConstraints, states, solverBodies, numRigidBodies, @@ -1002,6 +1015,8 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int int sz7 = sizeof(TrbState); */ + btPersistentManifold* offsetContactManifolds= (btPersistentManifold*) dispatcher->getInternalManifoldPool()->getPoolAddress(); + m_memoryCache->m_mysolverbodies.resize(numRigidBodies); m_memoryCache->m_mystates.resize(numRigidBodies); @@ -1122,8 +1137,9 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int pfxSetActive(pair,numPosPoints>0); pfxSetBroadphaseFlag(pair,0); + int contactId = m-offsetContactManifolds; - pfxSetContactId(pair,(uint64_t)m);//contactId); + pfxSetContactId(pair,contactId); pfxSetNumConstraints(pair,numPosPoints);//manifoldPtr[i]->getNumContacts()); actualNumManifolds++; } @@ -1136,10 +1152,13 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int int actualNumJoints=0; + btSolverConstraint* offsetSolverConstraints = 0; + //if (1) { - BT_PROFILE("convert constraints"); + { + BT_PROFILE("convert constraints"); int totalNumRows = 0; int i; @@ -1153,6 +1172,7 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int totalNumRows += info1.m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.resize(totalNumRows); + offsetSolverConstraints = &m_tmpSolverNonContactConstraintPool[0]; ///setup the btSolverConstraints @@ -1281,7 +1301,8 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask()); pfxSetActive(pair,true); - pfxSetContactId(pair,(uint64_t)currentConstraintRow);//contactId); + int id = currentConstraintRow-offsetSolverConstraints; + pfxSetContactId(pair,id); actualNumJoints++; @@ -1326,13 +1347,17 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int // PFX_PRINTF("num points = %d\n",totalPoints); // PFX_PRINTF("num points PFX = %d\n",total); + + BPE_customConstraintSolverSequentialNew( actualNumManifolds, &m_memoryCache->m_mypairs[0], + offsetContactManifolds, &m_memoryCache->m_mystates[0],numRigidBodies, &m_memoryCache->m_mysolverbodies[0], - 0,//manifoldArray, - jointPairs,actualNumJoints,separateBias,timeStep,iteration, + jointPairs,actualNumJoints, + offsetSolverConstraints, + separateBias,timeStep,iteration, m_solverThreadSupport,m_criticalSection,m_solverIO,m_barrier); } } diff --git a/src/BulletMultiThreaded/btParallelConstraintSolver.h b/src/BulletMultiThreaded/btParallelConstraintSolver.h index 10f35ab21..7c0268e7f 100644 --- a/src/BulletMultiThreaded/btParallelConstraintSolver.h +++ b/src/BulletMultiThreaded/btParallelConstraintSolver.h @@ -197,6 +197,7 @@ enum { struct PfxSetupContactConstraintsIO { PfxConstraintPair *offsetContactPairs; uint32_t numContactPairs1; + btPersistentManifold* offsetContactManifolds; class TrbState *offsetRigStates; struct PfxSolverBody *offsetSolverBodies; uint32_t numRigidBodies; @@ -217,7 +218,8 @@ struct PfxSolveConstraintsIO { PfxParallelBatch *jointParallelBatches; PfxConstraintPair *jointPairs; uint32_t numJointPairs; - TrbState *offsetRigStates; + struct btSolverConstraint* offsetSolverConstraints; + TrbState *offsetRigStates1; PfxSolverBody *offsetSolverBodies; uint32_t numRigidBodies; uint32_t iteration;