1 files changed, 140 insertions, 204 deletions

diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
index 1d10bad922..8207b47135 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btDiscreteDynamicsWorldMt.h"
 
 //collision detection
@@ -38,290 +37,227 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
-
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 
-
 #include "BulletDynamics/Dynamics/btActionInterface.h"
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btMotionState.h"
 
 #include "LinearMath/btSerializer.h"
 
-
-struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback
-{
-	btContactSolverInfo*	m_solverInfo;
-	btConstraintSolver*		m_solver;
-	btIDebugDraw*			m_debugDrawer;
-	btDispatcher*			m_dispatcher;
-
-	InplaceSolverIslandCallbackMt(
-		btConstraintSolver*	solver,
-		btStackAlloc* stackAlloc,
-		btDispatcher* dispatcher)
-		:m_solverInfo(NULL),
-		m_solver(solver),
-		m_debugDrawer(NULL),
-		m_dispatcher(dispatcher)
-	{
-
-	}
-
-	InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other)
-	{
-		btAssert(0);
-		(void)other;
-		return *this;
-	}
-
-	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer)
-	{
-		btAssert(solverInfo);
-		m_solverInfo = solverInfo;
-		m_debugDrawer = debugDrawer;
-	}
-
-
-	virtual	void	processIsland( btCollisionObject** bodies,
-                                   int numBodies,
-                                   btPersistentManifold** manifolds,
-                                   int numManifolds,
-                                   btTypedConstraint** constraints,
-                                   int numConstraints,
-                                   int islandId
-                                   )
-	{
-        m_solver->solveGroup( bodies,
-                              numBodies,
-                              manifolds,
-                              numManifolds,
-                              constraints,
-                              numConstraints,
-                              *m_solverInfo,
-                              m_debugDrawer,
-                              m_dispatcher
-                              );
-    }
-
-};
-
-
 ///
 /// btConstraintSolverPoolMt
 ///
 
 btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver()
 {
-    int i = 0;
+	int i = 0;
 #if BT_THREADSAFE
-    i = btGetCurrentThreadIndex() % m_solvers.size();
-#endif // #if BT_THREADSAFE
-    while ( true )
-    {
-        ThreadSolver& solver = m_solvers[ i ];
-        if ( solver.mutex.tryLock() )
-        {
-            return &solver;
-        }
-        // failed, try the next one
-        i = ( i + 1 ) % m_solvers.size();
-    }
-    return NULL;
+	i = btGetCurrentThreadIndex() % m_solvers.size();
+#endif  // #if BT_THREADSAFE
+	while (true)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		if (solver.mutex.tryLock())
+		{
+			return &solver;
+		}
+		// failed, try the next one
+		i = (i + 1) % m_solvers.size();
+	}
+	return NULL;
 }
 
-void btConstraintSolverPoolMt::init( btConstraintSolver** solvers, int numSolvers )
+void btConstraintSolverPoolMt::init(btConstraintSolver** solvers, int numSolvers)
 {
-    m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
-    m_solvers.resize( numSolvers );
-    for ( int i = 0; i < numSolvers; ++i )
-    {
-        m_solvers[ i ].solver = solvers[ i ];
-    }
-    if ( numSolvers > 0 )
-    {
-        m_solverType = solvers[ 0 ]->getSolverType();
-    }
+	m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
+	m_solvers.resize(numSolvers);
+	for (int i = 0; i < numSolvers; ++i)
+	{
+		m_solvers[i].solver = solvers[i];
+	}
+	if (numSolvers > 0)
+	{
+		m_solverType = solvers[0]->getSolverType();
+	}
 }
 
 // create the solvers for me
-btConstraintSolverPoolMt::btConstraintSolverPoolMt( int numSolvers )
+btConstraintSolverPoolMt::btConstraintSolverPoolMt(int numSolvers)
 {
-    btAlignedObjectArray<btConstraintSolver*> solvers;
-    solvers.reserve( numSolvers );
-    for ( int i = 0; i < numSolvers; ++i )
-    {
-        btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
-        solvers.push_back( solver );
-    }
-    init( &solvers[ 0 ], numSolvers );
+	btAlignedObjectArray<btConstraintSolver*> solvers;
+	solvers.reserve(numSolvers);
+	for (int i = 0; i < numSolvers; ++i)
+	{
+		btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
+		solvers.push_back(solver);
+	}
+	init(&solvers[0], numSolvers);
 }
 
 // pass in fully constructed solvers (destructor will delete them)
-btConstraintSolverPoolMt::btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers )
+btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers)
 {
-    init( solvers, numSolvers );
+	init(solvers, numSolvers);
 }
 
 btConstraintSolverPoolMt::~btConstraintSolverPoolMt()
 {
-    // delete all solvers
-    for ( int i = 0; i < m_solvers.size(); ++i )
-    {
-        ThreadSolver& solver = m_solvers[ i ];
-        delete solver.solver;
-        solver.solver = NULL;
-    }
+	// delete all solvers
+	for (int i = 0; i < m_solvers.size(); ++i)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		delete solver.solver;
+		solver.solver = NULL;
+	}
 }
 
 ///solve a group of constraints
-btScalar btConstraintSolverPoolMt::solveGroup( btCollisionObject** bodies,
-    int numBodies,
-    btPersistentManifold** manifolds,
-    int numManifolds,
-    btTypedConstraint** constraints,
-    int numConstraints,
-    const btContactSolverInfo& info,
-    btIDebugDraw* debugDrawer,
-    btDispatcher* dispatcher
-)
+btScalar btConstraintSolverPoolMt::solveGroup(btCollisionObject** bodies,
+											  int numBodies,
+											  btPersistentManifold** manifolds,
+											  int numManifolds,
+											  btTypedConstraint** constraints,
+											  int numConstraints,
+											  const btContactSolverInfo& info,
+											  btIDebugDraw* debugDrawer,
+											  btDispatcher* dispatcher)
 {
-    ThreadSolver* ts = getAndLockThreadSolver();
-    ts->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher );
-    ts->mutex.unlock();
-    return 0.0f;
+	ThreadSolver* ts = getAndLockThreadSolver();
+	ts->solver->solveGroup(bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
+	ts->mutex.unlock();
+	return 0.0f;
 }
 
 void btConstraintSolverPoolMt::reset()
 {
-    for ( int i = 0; i < m_solvers.size(); ++i )
-    {
-        ThreadSolver& solver = m_solvers[ i ];
-        solver.mutex.lock();
-        solver.solver->reset();
-        solver.mutex.unlock();
-    }
+	for (int i = 0; i < m_solvers.size(); ++i)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		solver.mutex.lock();
+		solver.solver->reset();
+		solver.mutex.unlock();
+	}
 }
 
-
 ///
 /// btDiscreteDynamicsWorldMt
 ///
 
-btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolverPoolMt* constraintSolver, btCollisionConfiguration* collisionConfiguration)
-: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
+													 btBroadphaseInterface* pairCache,
+													 btConstraintSolverPoolMt* solverPool,
+													 btConstraintSolver* constraintSolverMt,
+													 btCollisionConfiguration* collisionConfiguration)
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration)
 {
 	if (m_ownsIslandManager)
 	{
 		m_islandManager->~btSimulationIslandManager();
-		btAlignedFree( m_islandManager);
+		btAlignedFree(m_islandManager);
 	}
-    {
-		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16);
-		m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher);
-    }
 	{
-		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16);
 		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
-        im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
-        m_islandManager = im;
+		im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize);
+		m_islandManager = im;
 	}
+	m_constraintSolverMt = constraintSolverMt;
 }
 
-
 btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
 {
-	if (m_solverIslandCallbackMt)
-	{
-		m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt();
-		btAlignedFree(m_solverIslandCallbackMt);
-	}
-	if (m_ownsConstraintSolver)
-	{
-		m_constraintSolver->~btConstraintSolver();
-		btAlignedFree(m_constraintSolver);
-	}
 }
 
-
 void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
 
-	m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 
 	/// solve all the constraints for this island
-    btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
-    im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
+	btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
+	btSimulationIslandManagerMt::SolverParams solverParams;
+	solverParams.m_solverPool = m_constraintSolver;
+	solverParams.m_solverMt = m_constraintSolverMt;
+	solverParams.m_solverInfo = &solverInfo;
+	solverParams.m_debugDrawer = m_debugDrawer;
+	solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
+	im->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams);
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 }
 
-
 struct UpdaterUnconstrainedMotion : public btIParallelForBody
 {
-    btScalar timeStep;
-    btRigidBody** rigidBodies;
+	btScalar timeStep;
+	btRigidBody** rigidBodies;
 
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        for ( int i = iBegin; i < iEnd; ++i )
-        {
-            btRigidBody* body = rigidBodies[ i ];
-            if ( !body->isStaticOrKinematicObject() )
-            {
-                //don't integrate/update velocities here, it happens in the constraint solver
-                body->applyDamping( timeStep );
-                body->predictIntegratedTransform( timeStep, body->getInterpolationWorldTransform() );
-            }
-        }
-    }
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btRigidBody* body = rigidBodies[i];
+			if (!body->isStaticOrKinematicObject())
+			{
+				//don't integrate/update velocities here, it happens in the constraint solver
+				body->applyDamping(timeStep);
+				body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
+			}
+		}
+	}
 };
 
-
-void btDiscreteDynamicsWorldMt::predictUnconstraintMotion( btScalar timeStep )
+void btDiscreteDynamicsWorldMt::predictUnconstraintMotion(btScalar timeStep)
 {
-    BT_PROFILE( "predictUnconstraintMotion" );
-    if ( m_nonStaticRigidBodies.size() > 0 )
-    {
-        UpdaterUnconstrainedMotion update;
-        update.timeStep = timeStep;
-        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
-        int grainSize = 50;  // num of iterations per task for task scheduler
-        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
-    }
+	BT_PROFILE("predictUnconstraintMotion");
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterUnconstrainedMotion update;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
 }
 
-
-void btDiscreteDynamicsWorldMt::createPredictiveContacts( btScalar timeStep )
+void btDiscreteDynamicsWorldMt::createPredictiveContacts(btScalar timeStep)
 {
-    BT_PROFILE( "createPredictiveContacts" );
-    releasePredictiveContacts();
-    if ( m_nonStaticRigidBodies.size() > 0 )
-    {
-        UpdaterCreatePredictiveContacts update;
-        update.world = this;
-        update.timeStep = timeStep;
-        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
-        int grainSize = 50;  // num of iterations per task for task scheduler
-        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
-    }
+	BT_PROFILE("createPredictiveContacts");
+	releasePredictiveContacts();
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterCreatePredictiveContacts update;
+		update.world = this;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
 }
 
-
-void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep )
+void btDiscreteDynamicsWorldMt::integrateTransforms(btScalar timeStep)
 {
-    BT_PROFILE( "integrateTransforms" );
-    if ( m_nonStaticRigidBodies.size() > 0 )
-    {
-        UpdaterIntegrateTransforms update;
-        update.world = this;
-        update.timeStep = timeStep;
-        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
-        int grainSize = 50;  // num of iterations per task for task scheduler
-        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
-    }
+	BT_PROFILE("integrateTransforms");
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterIntegrateTransforms update;
+		update.world = this;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
 }
 
+int btDiscreteDynamicsWorldMt::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
+{
+	int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
+	if (btITaskScheduler* scheduler = btGetTaskScheduler())
+	{
+		// tell Bullet's threads to sleep, so other threads can run
+		scheduler->sleepWorkerThreadsHint();
+	}
+	return numSubSteps;
+}