MultiThreaded Demo:

- fixing various race conditions throughout (usage of static vars, etc)
 - addition of a few lightweight mutexes (which are compiled out by default)
 - slight code rearrangement in discreteDynamicsWorld to facilitate multithreading
 - PoolAllocator::allocate() can now be called when pool is full without
     crashing (null pointer returned)
 - PoolAllocator allocate and freeMemory, are OPTIONALLY threadsafe
     (default is un-threadsafe)
 - CollisionDispatcher no longer checks if the pool allocator is full
     before calling allocate(), instead it just calls allocate() and
     checks if the return is null -- this avoids a race condition
 - SequentialImpulseConstraintSolver OPTIONALLY uses different logic in
     getOrInitSolverBody() to avoid a race condition with kinematic bodies
 - addition of 2 classes which together allow simulation islands to be run
   in parallel:
    - btSimulationIslandManagerMt
    - btDiscreteDynamicsWorldMt
 - MultiThreadedDemo example in the example browser demonstrating use of
   OpenMP, Microsoft PPL, and Intel TBB
 - use multithreading for other demos
 - benchmark demo: add parallel raycasting

CMakeLists.txt

@@ -28,6 +28,7 @@ OPTION(USE_DOUBLE_PRECISION "Use double precision"	OFF)
 OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
 OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
 OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF)	
+OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations" OFF)
 
 OPTION(USE_MSVC_INCREMENTAL_LINKING "Use MSVC Incremental Linking" OFF)
 OPTION(USE_CUSTOM_VECTOR_MATH "Use custom vectormath library" OFF)
@@ -155,6 +156,13 @@ IF(USE_GRAPHICAL_BENCHMARK)
 ADD_DEFINITIONS( -DUSE_GRAPHICAL_BENCHMARK)
 ENDIF (USE_GRAPHICAL_BENCHMARK)
 
+IF(BULLET2_USE_THREAD_LOCKS)
+	ADD_DEFINITIONS( -DBT_THREADSAFE=1 )
+	IF (NOT MSVC)
+		SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	ENDIF (NOT MSVC)
+ENDIF (BULLET2_USE_THREAD_LOCKS)
+
 IF (WIN32)
 OPTION(USE_GLUT "Use Glut"	ON)
 ADD_DEFINITIONS( -D_CRT_SECURE_NO_WARNINGS )
@@ -195,8 +203,6 @@ ENDIF (OPENGL_FOUND)
 #FIND_PACKAGE(GLU)
 
 
-
-
 IF (APPLE)
   FIND_LIBRARY(COCOA_LIBRARY Cocoa)
 ENDIF()
@@ -264,6 +270,15 @@ IF(BUILD_BULLET2_DEMOS)
 	IF(EXISTS ${BULLET_PHYSICS_SOURCE_DIR}/examples AND IS_DIRECTORY ${BULLET_PHYSICS_SOURCE_DIR}/examples)
 		SUBDIRS(examples)
 	ENDIF()
+
+        IF (BULLET2_USE_THREAD_LOCKS)
+            OPTION(BULLET2_MULTITHREADED_OPEN_MP_DEMO "Build Bullet 2 MultithreadedDemo using OpenMP (requires a compiler with OpenMP support)" OFF)
+            OPTION(BULLET2_MULTITHREADED_TBB_DEMO "Build Bullet 2 MultithreadedDemo using Intel Threading Building Blocks (requires the TBB library to be already installed)" OFF)
+            IF (MSVC)
+                OPTION(BULLET2_MULTITHREADED_PPL_DEMO "Build Bullet 2 MultithreadedDemo using Microsoft Parallel Patterns Library (requires MSVC compiler)" OFF)
+            ENDIF (MSVC)
+        ENDIF (BULLET2_USE_THREAD_LOCKS)
+
 ENDIF(BUILD_BULLET2_DEMOS)
 
 

examples/BasicDemo/CMakeLists.txt

@@ -12,12 +12,14 @@ IF (WIN32)
 	ADD_EXECUTABLE(App_BasicExample
 		BasicExample.cpp
 		main.cpp
+		../CommonInterfaces/CommonRigidBodyBase.cpp
 		${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc
 	)
 ELSE()
 	ADD_EXECUTABLE(App_BasicExample
 		BasicExample.cpp
 		main.cpp
+		../CommonInterfaces/CommonRigidBodyBase.cpp
 	)
 ENDIF()
 
@@ -53,6 +55,7 @@ SET(AppBasicExampleGui_SRCS
         ../ExampleBrowser/OpenGLGuiHelper.cpp
         ../ExampleBrowser/GL_ShapeDrawer.cpp
         ../ExampleBrowser/CollisionShape2TriangleMesh.cpp
+		../CommonInterfaces/CommonRigidBodyBase.cpp
 		../Utils/b3Clock.cpp
 )
 

examples/BasicDemo/premake4.lua

@@ -18,6 +18,7 @@ language "C++"
 files {
 	"**.cpp",
 	"**.h",
+	"../CommonInterfaces/*",
 }
 
 
@@ -49,6 +50,7 @@ files {
 	"../ExampleBrowser/OpenGLGuiHelper.cpp",
 	"../ExampleBrowser/GL_ShapeDrawer.cpp",
 	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+	"../CommonInterfaces/*",
 	"../Utils/b3Clock.cpp",
 	"../Utils/b3Clock.h",
 }
@@ -90,6 +92,7 @@ files {
 	"../ExampleBrowser/OpenGLGuiHelper.cpp",
 	"../ExampleBrowser/GL_ShapeDrawer.cpp",
 	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+	"../CommonInterfaces/*",
 	"../TinyRenderer/geometry.cpp",
 	"../TinyRenderer/model.cpp",
 	"../TinyRenderer/tgaimage.cpp",
@@ -130,6 +133,7 @@ files {
         "*.h",
         "../StandaloneMain/main_tinyrenderer_single_example.cpp",
 	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+	"../CommonInterfaces/*",
 	"../OpenGLWindow/SimpleCamera.cpp",
 	"../TinyRenderer/geometry.cpp",
 	"../TinyRenderer/model.cpp",
@@ -175,6 +179,7 @@ files {
         "BasicExample.cpp",
         "*.h",
         "../StandaloneMain/hellovr_opengl_main.cpp",
+	"../CommonInterfaces/*",
 				"../ExampleBrowser/OpenGLGuiHelper.cpp",
 				"../ExampleBrowser/GL_ShapeDrawer.cpp",
 				"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",

examples/Benchmarks/BenchmarkDemo.cpp

@@ -32,10 +32,12 @@ subject to the following restrictions:
 
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btTransform.h"
+#include "../CommonInterfaces/ParallelFor.h"
 
 class btDynamicsWorld;
 
 #define NUMRAYS 500
+#define USE_PARALLEL_RAYCASTS 1
 
 class btRigidBody;
 class btBroadphaseInterface;
@@ -204,7 +206,39 @@ public:
 			sign = -1.0;
 	}
 
-	void cast (btCollisionWorld* cw)
+    void castRays( btCollisionWorld* cw, int iBegin, int iEnd )
+    {
+        for ( int i = iBegin; i < iEnd; ++i )
+        {
+			btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]);
+			
+			cw->rayTest (source[i], dest[i], cb);
+			if (cb.hasHit ())
+			{
+				hit[i] = cb.m_hitPointWorld;
+				normal[i] = cb.m_hitNormalWorld;
+				normal[i].normalize ();
+			} else {
+				hit[i] = dest[i];
+				normal[i] = btVector3(1.0, 0.0, 0.0);
+			}
+
+        }
+    }
+
+    struct CastRaysLoopBody
+    {
+        btRaycastBar2* mRaycasts;
+        btCollisionWorld* mWorld;
+        CastRaysLoopBody(btCollisionWorld* cw, btRaycastBar2* rb) : mWorld(cw), mRaycasts(rb) {}
+
+        void forLoop( int iBegin, int iEnd ) const
+        {
+            mRaycasts->castRays(mWorld, iBegin, iEnd);
+        }
+    };
+
+	void cast (btCollisionWorld* cw, bool multiThreading = false)
 	{
 #ifdef USE_BT_CLOCK
 		frame_timer.reset ();
@@ -228,21 +262,18 @@ public:
 				normal[i].normalize ();
 		}
 #else
-		for (int i = 0; i < NUMRAYS; i++)
+#if USE_PARALLEL_RAYCASTS
+        if ( multiThreading )
         {
-			btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]);
+            CastRaysLoopBody rayLooper(cw, this);
-			
+            int grainSize = 20;  // number of raycasts per task
-			cw->rayTest (source[i], dest[i], cb);
+            parallelFor( 0, NUMRAYS, grainSize, rayLooper );
-			if (cb.hasHit ())
-			{
-				hit[i] = cb.m_hitPointWorld;
-				normal[i] = cb.m_hitNormalWorld;
-				normal[i].normalize ();
-			} else {
-				hit[i] = dest[i];
-				normal[i] = btVector3(1.0, 0.0, 0.0);
         }
-
+        else
+#endif // USE_PARALLEL_RAYCASTS
+        {
+            // single threaded
+            castRays(cw, 0, NUMRAYS);
         }
 #ifdef USE_BT_CLOCK
 		ms += frame_timer.getTimeMilliseconds ();
@@ -354,42 +385,43 @@ void	BenchmarkDemo::initPhysics()
 
 	setCameraDistance(btScalar(100.));
 
-	///collision configuration contains default setup for memory, collision setup
+    createEmptyDynamicsWorld();
-	btDefaultCollisionConstructionInfo cci;
+	/////collision configuration contains default setup for memory, collision setup
-	cci.m_defaultMaxPersistentManifoldPoolSize = 32768;
+	//btDefaultCollisionConstructionInfo cci;
-	m_collisionConfiguration = new btDefaultCollisionConfiguration(cci);
+	//cci.m_defaultMaxPersistentManifoldPoolSize = 32768;
+	//m_collisionConfiguration = new btDefaultCollisionConfiguration(cci);
 
-	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	/////use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
-	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
+	//m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
-	
+	//
-	m_dispatcher->setDispatcherFlags(btCollisionDispatcher::CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION);
+	//m_dispatcher->setDispatcherFlags(btCollisionDispatcher::CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION);
 
 
 
-	///the maximum size of the collision world. Make sure objects stay within these boundaries
+	/////the maximum size of the collision world. Make sure objects stay within these boundaries
-	///Don't make the world AABB size too large, it will harm simulation quality and performance
+	/////Don't make the world AABB size too large, it will harm simulation quality and performance
-	btVector3 worldAabbMin(-1000,-1000,-1000);
+	//btVector3 worldAabbMin(-1000,-1000,-1000);
-	btVector3 worldAabbMax(1000,1000,1000);
+	//btVector3 worldAabbMax(1000,1000,1000);
-	
+	//
-	btHashedOverlappingPairCache* pairCache = new btHashedOverlappingPairCache();
+	//btHashedOverlappingPairCache* pairCache = new btHashedOverlappingPairCache();
-	m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,3500,pairCache);
+	//m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,3500,pairCache);
 //	m_broadphase = new btSimpleBroadphase();
 //	m_broadphase = new btDbvtBroadphase();
 	
 
 	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
-	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
+	//btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
 	
 	
-	m_solver = sol;
+	//m_solver = sol;
 
-	btDiscreteDynamicsWorld* dynamicsWorld;
+	//btDiscreteDynamicsWorld* dynamicsWorld;
-	m_dynamicsWorld = dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
+	//m_dynamicsWorld = dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
 	
 
 	///the following 3 lines increase the performance dramatically, with a little bit of loss of quality
 	m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_ENABLE_FRICTION_DIRECTION_CACHING; //don't recalculate friction values each frame
-	dynamicsWorld->getSolverInfo().m_numIterations = 5; //few solver iterations 
+	m_dynamicsWorld->getSolverInfo().m_numIterations = 5; //few solver iterations 
 	//m_defaultContactProcessingThreshold = 0.f;//used when creating bodies: body->setContactProcessingThreshold(...);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 	
@@ -1242,7 +1274,7 @@ void BenchmarkDemo::initRays()
 
 void BenchmarkDemo::castRays()
 {
-	raycastBar.cast (m_dynamicsWorld);
+	raycastBar.cast (m_dynamicsWorld, m_multithreadedWorld);
 }
 
 void	BenchmarkDemo::createTest7()

examples/CommonInterfaces/CommonGraphicsAppInterface.h

@@ -120,7 +120,7 @@ struct CommonGraphicsApp
 	virtual int getUpAxis() const = 0;
 	
 	virtual void swapBuffer() = 0;
-	virtual void drawText( const char* txt, int posX, int posY) = 0;
+	virtual void drawText( const char* txt, int posX, int posY, float size = 1.0f) = 0;
 	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
 	virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)=0;
 	virtual int	registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ, int textureIndex = -1,  float textureScaling = 1)=0;

examples/CommonInterfaces/CommonRigidBodyBase.cpp

@@ -0,0 +1,855 @@
+/*
+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 "btBulletDynamicsCommon.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#include <stdio.h>
+#include <algorithm>
+
+class btCollisionShape;
+
+#include "CommonExampleInterface.h"
+#include "CommonRigidBodyBase.h"
+#include "CommonParameterInterface.h"
+#include "CommonGUIHelperInterface.h"
+#include "CommonRenderInterface.h"
+#include "CommonWindowInterface.h"
+#include "CommonGraphicsAppInterface.h"
+#include "ParallelFor.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btPoolAllocator.h"
+#include "btBulletCollisionCommon.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManagerMt.h"  // for setSplitIslands()
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+
+TaskManager gTaskMgr;
+
+#define USE_PARALLEL_NARROWPHASE 1  // detect collisions in parallel
+#define USE_PARALLEL_ISLAND_SOLVER 1   // solve simulation islands in parallel
+#define USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS 1
+#define USE_PARALLEL_INTEGRATE_TRANSFORMS 1
+#define USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION 1
+
+#if defined (_MSC_VER) && _MSC_VER >= 1600
+// give us a compile error if any signatures of overriden methods is changed
+#define BT_OVERRIDE override
+#else
+#define BT_OVERRIDE
+#endif
+
+
+class Profiler
+{
+public:
+    enum RecordType
+    {
+        kRecordInternalTimeStep,
+        kRecordDispatchAllCollisionPairs,
+        kRecordDispatchIslands,
+        kRecordPredictUnconstrainedMotion,
+        kRecordCreatePredictiveContacts,
+        kRecordIntegrateTransforms,
+        kRecordCount
+    };
+
+private:
+    btClock mClock;
+
+    struct Record
+    {
+        int mCallCount;
+        unsigned long long mAccum;
+        unsigned int mStartTime;
+        unsigned int mHistory[8];
+
+        void begin(unsigned int curTime)
+        {
+            mStartTime = curTime;
+        }
+        void end(unsigned int curTime)
+        {
+            unsigned int endTime = curTime;
+            unsigned int elapsed = endTime - mStartTime;
+            mAccum += elapsed;
+            mHistory[ mCallCount & 7 ] = elapsed;
+            ++mCallCount;
+        }
+        float getAverageTime() const
+        {
+            int count = btMin( 8, mCallCount );
+            if ( count > 0 )
+            {
+                unsigned int sum = 0;
+                for ( int i = 0; i < count; ++i )
+                {
+                    sum += mHistory[ i ];
+                }
+                float avg = float( sum ) / float( count );
+                return avg;
+            }
+            return 0.0;
+        }
+    };
+    Record mRecords[ kRecordCount ];
+
+public:
+    void begin(RecordType rt)
+    {
+        mRecords[rt].begin(mClock.getTimeMicroseconds());
+    }
+    void end(RecordType rt)
+    {
+        mRecords[rt].end(mClock.getTimeMicroseconds());
+    }
+    float getAverageTime(RecordType rt) const
+    {
+        return mRecords[rt].getAverageTime();
+    }
+};
+
+
+Profiler gProfiler;
+
+class ProfileHelper
+{
+    Profiler::RecordType mRecType;
+public:
+    ProfileHelper(Profiler::RecordType rt)
+    {
+        mRecType = rt;
+        gProfiler.begin( mRecType );
+    }
+    ~ProfileHelper()
+    {
+        gProfiler.end( mRecType );
+    }
+};
+
+int gThreadsRunningCounter = 0;
+btSpinMutex gThreadsRunningCounterMutex;
+
+void btPushThreadsAreRunning()
+{
+    gThreadsRunningCounterMutex.lock();
+    gThreadsRunningCounter++;
+    gThreadsRunningCounterMutex.unlock();
+}
+
+void btPopThreadsAreRunning()
+{
+    gThreadsRunningCounterMutex.lock();
+    gThreadsRunningCounter--;
+    gThreadsRunningCounterMutex.unlock();
+}
+
+bool btThreadsAreRunning()
+{
+    return gThreadsRunningCounter != 0;
+}
+
+
+#if USE_PARALLEL_NARROWPHASE
+
+class MyCollisionDispatcher : public btCollisionDispatcher
+{
+    btSpinMutex m_manifoldPtrsMutex;
+
+public:
+    MyCollisionDispatcher( btCollisionConfiguration* config ) : btCollisionDispatcher( config )
+    {
+    }
+
+    virtual ~MyCollisionDispatcher()
+    {
+    }
+
+    btPersistentManifold* getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) BT_OVERRIDE
+    {
+        // added spin-locks
+        //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
+
+        btScalar contactBreakingThreshold = ( m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD ) ?
+            btMin( body0->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ), body1->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ) )
+            : gContactBreakingThreshold;
+
+        btScalar contactProcessingThreshold = btMin( body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold() );
+
+        void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) );
+        if (NULL == mem)
+        {
+            //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 );
+        m_manifoldPtrsMutex.lock();
+        manifold->m_index1a = m_manifoldsPtr.size();
+        m_manifoldsPtr.push_back( manifold );
+        m_manifoldPtrsMutex.unlock();
+
+        return manifold;
+    }
+
+    void releaseManifold( btPersistentManifold* manifold ) BT_OVERRIDE
+    {
+        clearManifold( manifold );
+
+        m_manifoldPtrsMutex.lock();
+        int findIndex = manifold->m_index1a;
+        btAssert( findIndex < m_manifoldsPtr.size() );
+        m_manifoldsPtr.swap( findIndex, m_manifoldsPtr.size() - 1 );
+        m_manifoldsPtr[ findIndex ]->m_index1a = findIndex;
+        m_manifoldsPtr.pop_back();
+        m_manifoldPtrsMutex.unlock();
+
+        manifold->~btPersistentManifold();
+        if ( m_persistentManifoldPoolAllocator->validPtr( manifold ) )
+        {
+            m_persistentManifoldPoolAllocator->freeMemory( manifold );
+        }
+        else
+        {
+            btAlignedFree( manifold );
+        }
+    }
+
+    struct Updater
+    {
+        btBroadphasePair* mPairArray;
+        btNearCallback mCallback;
+        btCollisionDispatcher* mDispatcher;
+        const btDispatcherInfo* mInfo;
+
+        Updater()
+        {
+            mPairArray = NULL;
+            mCallback = NULL;
+            mDispatcher = NULL;
+            mInfo = NULL;
+        }
+        void forLoop( int iBegin, int iEnd ) const
+        {
+            for ( int i = iBegin; i < iEnd; ++i )
+            {
+                btBroadphasePair* pair = &mPairArray[ i ];
+                mCallback( *pair, *mDispatcher, *mInfo );
+            }
+        }
+    };
+
+    virtual void dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) BT_OVERRIDE
+    {
+        ProfileHelper prof(Profiler::kRecordDispatchAllCollisionPairs);
+        int grainSize = 40;  // iterations per task
+        int pairCount = pairCache->getNumOverlappingPairs();
+        Updater updater;
+        updater.mCallback = getNearCallback();
+        updater.mPairArray = pairCount > 0 ? pairCache->getOverlappingPairArrayPtr() : NULL;
+        updater.mDispatcher = this;
+        updater.mInfo = &info;
+
+        btPushThreadsAreRunning();
+        parallelFor( 0, pairCount, grainSize, updater );
+        btPopThreadsAreRunning();
+        
+        if (m_manifoldsPtr.size() < 1)
+            return;
+
+        // reconstruct the manifolds array to ensure determinism
+        m_manifoldsPtr.resizeNoInitialize(0);
+        btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
+        for (int i = 0; i < pairCount; ++i)
+        {
+            btCollisionAlgorithm* algo = pairs[i].m_algorithm;
+            if (algo) algo->getAllContactManifolds(m_manifoldsPtr);
+        }
+
+        // update the indices (used when releasing manifolds)
+        for (int i = 0; i < m_manifoldsPtr.size(); ++i)
+            m_manifoldsPtr[i]->m_index1a = i;
+    }
+};
+
+#endif
+
+
+#if USE_PARALLEL_ISLAND_SOLVER
+///
+/// MyConstraintSolverPool - masquerades as a constraint solver, but really it is a threadsafe pool of them.
+///
+///  Each solver in the pool is protected by a mutex.  When solveGroup is called from a thread,
+///  the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the
+///  call to the solver.
+///  So long as there are at least as many solvers as there are hardware threads, it should never need to
+///  spin wait.
+///
+class MyConstraintSolverPool : public btConstraintSolver
+{
+    const static size_t kCacheLineSize = 128;
+    struct ThreadSolver
+    {
+        btConstraintSolver* solver;
+        btSpinMutex mutex;
+        char _cachelinePadding[ kCacheLineSize - sizeof( btSpinMutex ) - sizeof( void* ) ];  // keep mutexes from sharing a cache line
+    };
+    btAlignedObjectArray<ThreadSolver> m_solvers;
+    btConstraintSolverType m_solverType;
+
+    ThreadSolver* getAndLockThreadSolver()
+    {
+        while ( true )
+        {
+            for ( int i = 0; i < m_solvers.size(); ++i )
+            {
+                ThreadSolver& solver = m_solvers[ i ];
+                if ( solver.mutex.tryLock() )
+                {
+                    return &solver;
+                }
+            }
+        }
+        return NULL;
+    }
+    void 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();
+        }
+    }
+public:
+    // create the solvers for me
+    explicit MyConstraintSolverPool( 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 );
+    }
+
+    // pass in fully constructed solvers (destructor will delete them)
+    MyConstraintSolverPool( btConstraintSolver** solvers, int numSolvers )
+    {
+        init( solvers, numSolvers );
+    }
+    virtual ~MyConstraintSolverPool()
+    {
+        // delete all solvers
+        for ( int i = 0; i < m_solvers.size(); ++i )
+        {
+            ThreadSolver& solver = m_solvers[ i ];
+            delete solver.solver;
+            solver.solver = NULL;
+        }
+    }
+
+    //virtual void prepareSolve( int /* numBodies */, int /* numManifolds */ ) { ; } // does nothing
+
+    ///solve a group of constraints
+    virtual btScalar solveGroup( btCollisionObject** bodies,
+                                 int numBodies,
+                                 btPersistentManifold** manifolds,
+                                 int numManifolds,
+                                 btTypedConstraint** constraints,
+                                 int numConstraints,
+                                 const btContactSolverInfo& info,
+                                 btIDebugDraw* debugDrawer,
+                                 btDispatcher* dispatcher
+                                 )
+    {
+        ThreadSolver* solver = getAndLockThreadSolver();
+        solver->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher );
+        solver->mutex.unlock();
+        return 0.0f;
+    }
+
+    //virtual void allSolved( const btContactSolverInfo& /* info */, class btIDebugDraw* /* debugDrawer */ ) { ; } // does nothing
+
+    ///clear internal cached data and reset random seed
+    virtual	void reset()
+    {
+        for ( int i = 0; i < m_solvers.size(); ++i )
+        {
+            ThreadSolver& solver = m_solvers[ i ];
+            solver.mutex.lock();
+            solver.solver->reset();
+            solver.mutex.unlock();
+        }
+    }
+
+    virtual btConstraintSolverType getSolverType() const
+    {
+        return m_solverType;
+    }
+};
+
+struct UpdateIslandDispatcher
+{
+    btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
+    btSimulationIslandManagerMt::IslandCallback* callback;
+
+    void forLoop( int iBegin, int iEnd ) const
+    {
+        for ( int i = iBegin; i < iEnd; ++i )
+        {
+            btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ];
+            btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
+            btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
+            callback->processIsland( &island->bodyArray[ 0 ],
+                                     island->bodyArray.size(),
+                                     manifolds,
+                                     island->manifoldArray.size(),
+                                     constraintsPtr,
+                                     island->constraintArray.size(),
+                                     island->id
+                                     );
+        }
+    }
+};
+
+static int gNumIslands = 0;
+
+void parallelIslandDispatch( btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr, btSimulationIslandManagerMt::IslandCallback* callback )
+{
+    ProfileHelper prof(Profiler::kRecordDispatchIslands);
+    gNumIslands = islandsPtr->size();
+    int grainSize = 1;  // iterations per task
+    UpdateIslandDispatcher dispatcher;
+    dispatcher.islandsPtr = islandsPtr;
+    dispatcher.callback = callback;
+    btPushThreadsAreRunning();
+    parallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
+    btPopThreadsAreRunning();
+}
+#endif //#if USE_PARALLEL_ISLAND_SOLVER
+
+
+void profileBeginCallback(btDynamicsWorld *world, btScalar timeStep)
+{
+    gProfiler.begin(Profiler::kRecordInternalTimeStep);
+}
+
+void profileEndCallback(btDynamicsWorld *world, btScalar timeStep)
+{
+    gProfiler.end(Profiler::kRecordInternalTimeStep);
+}
+
+///
+/// MyDiscreteDynamicsWorld
+///
+///  Should function exactly like btDiscreteDynamicsWorld.
+///  3 methods that iterate over all of the rigidbodies can run in parallel:
+///     - predictUnconstraintMotion
+///     - integrateTransforms
+///     - createPredictiveContacts
+///
+ATTRIBUTE_ALIGNED16( class ) MyDiscreteDynamicsWorld : public btDiscreteDynamicsWorldMt
+{
+    typedef btDiscreteDynamicsWorld ParentClass;
+
+protected:
+#if USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION
+    struct UpdaterUnconstrainedMotion
+    {
+        btScalar timeStep;
+        btRigidBody** rigidBodies;
+
+        void forLoop( int iBegin, int iEnd ) const
+        {
+            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() );
+                }
+            }
+        }
+    };
+
+    virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE
+    {
+        ProfileHelper prof( Profiler::kRecordPredictUnconstrainedMotion );
+        BT_PROFILE( "predictUnconstraintMotion" );
+        int grainSize = 50;  // num of iterations per task for TBB
+        int bodyCount = m_nonStaticRigidBodies.size();
+        UpdaterUnconstrainedMotion update;
+        update.timeStep = timeStep;
+        update.rigidBodies = bodyCount ? &m_nonStaticRigidBodies[ 0 ] : NULL;
+        btPushThreadsAreRunning();
+        parallelFor( 0, bodyCount, grainSize, update );
+        btPopThreadsAreRunning();
+    }
+#endif // #if USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION
+
+#if USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS
+    struct UpdaterCreatePredictiveContacts
+    {
+        btScalar timeStep;
+        btRigidBody** rigidBodies;
+        MyDiscreteDynamicsWorld* world;
+
+        void forLoop( int iBegin, int iEnd ) const
+        {
+            world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
+        }
+    };
+
+    virtual void createPredictiveContacts( btScalar timeStep )
+    {
+        ProfileHelper prof( Profiler::kRecordCreatePredictiveContacts );
+        releasePredictiveContacts();
+        int grainSize = 50;  // num of iterations per task for TBB or OPENMP
+        if ( int bodyCount = m_nonStaticRigidBodies.size() )
+        {
+            UpdaterCreatePredictiveContacts update;
+            update.world = this;
+            update.timeStep = timeStep;
+            update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
+            btPushThreadsAreRunning();
+            parallelFor( 0, bodyCount, grainSize, update );
+            btPopThreadsAreRunning();
+        }
+    }
+#endif // #if USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS
+
+#if USE_PARALLEL_INTEGRATE_TRANSFORMS
+    struct UpdaterIntegrateTransforms
+    {
+        btScalar timeStep;
+        btRigidBody** rigidBodies;
+        MyDiscreteDynamicsWorld* world;
+
+        void forLoop( int iBegin, int iEnd ) const
+        {
+            world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
+        }
+    };
+
+    virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE
+    {
+        ProfileHelper prof( Profiler::kRecordIntegrateTransforms );
+        BT_PROFILE( "integrateTransforms" );
+        int grainSize = 50;  // num of iterations per task for TBB or OPENMP
+        if ( int bodyCount = m_nonStaticRigidBodies.size() )
+        {
+            UpdaterIntegrateTransforms update;
+            update.world = this;
+            update.timeStep = timeStep;
+            update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
+            btPushThreadsAreRunning();
+            parallelFor( 0, bodyCount, grainSize, update );
+            btPopThreadsAreRunning();
+        }
+    }
+#endif // #if USE_PARALLEL_INTEGRATE_TRANSFORMS
+
+public:
+    BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    MyDiscreteDynamicsWorld( btDispatcher* dispatcher,
+                             btBroadphaseInterface* pairCache,
+                             btConstraintSolver* constraintSolver,
+                             btCollisionConfiguration* collisionConfiguration
+                             ) :
+                             btDiscreteDynamicsWorldMt( dispatcher, pairCache, constraintSolver, collisionConfiguration )
+    {
+    }
+
+};
+
+static bool gMultithreadedWorld = false;
+static bool gDisplayProfileInfo = false;
+static btScalar gSliderNumThreads = 1.0f;  // should be int
+
+////////////////////////////////////
+CommonRigidBodyBase::CommonRigidBodyBase( struct GUIHelperInterface* helper )
+    :m_broadphase( 0 ),
+    m_dispatcher( 0 ),
+    m_solver( 0 ),
+    m_collisionConfiguration( 0 ),
+    m_dynamicsWorld( 0 ),
+    m_pickedBody( 0 ),
+    m_pickedConstraint( 0 ),
+    m_guiHelper( helper )
+{
+    m_multithreadedWorld = false;
+    m_multithreadCapable = false;
+    gTaskMgr.init();
+}
+
+CommonRigidBodyBase::~CommonRigidBodyBase()
+{
+    gTaskMgr.shutdown();
+}
+
+void boolPtrButtonCallback(int buttonId, bool buttonState, void* userPointer)
+{
+    if (bool* val = static_cast<bool*>(userPointer))
+    {
+        *val = ! *val;
+    }
+}
+
+void apiSelectButtonCallback(int buttonId, bool buttonState, void* userPointer)
+{
+    gTaskMgr.setApi(static_cast<TaskManager::Api>(buttonId));
+    if (gTaskMgr.getApi()==TaskManager::apiNone)
+    {
+        gSliderNumThreads = 1.0f;
+    }
+    else
+    {
+        gSliderNumThreads = float(gTaskMgr.getNumThreads());
+    }
+}
+
+void setThreadCountCallback(float val)
+{
+    if (gTaskMgr.getApi()==TaskManager::apiNone)
+    {
+        gSliderNumThreads = 1.0f;
+    }
+    else
+    {
+        gTaskMgr.setNumThreads( int( gSliderNumThreads ) );
+        gSliderNumThreads = float(gTaskMgr.getNumThreads());
+    }
+}
+
+void CommonRigidBodyBase::createEmptyDynamicsWorld()
+{
+    gNumIslands = 0;
+#if BT_THREADSAFE && (BT_USE_OPENMP || BT_USE_PPL || BT_USE_TBB)
+    m_multithreadCapable = true;
+#endif
+    if ( gMultithreadedWorld )
+    {
+        m_dispatcher = NULL;
+        btDefaultCollisionConstructionInfo cci;
+        cci.m_defaultMaxPersistentManifoldPoolSize = 80000;
+        cci.m_defaultMaxCollisionAlgorithmPoolSize = 80000;
+        m_collisionConfiguration = new btDefaultCollisionConfiguration( cci );
+
+#if USE_PARALLEL_NARROWPHASE
+        m_dispatcher = new	MyCollisionDispatcher( m_collisionConfiguration );
+#else
+        m_dispatcher = new	btCollisionDispatcher( m_collisionConfiguration );
+#endif //USE_PARALLEL_NARROWPHASE
+
+        m_broadphase = new btDbvtBroadphase();
+
+#if USE_PARALLEL_ISLAND_SOLVER
+        m_solver = new MyConstraintSolverPool( TaskManager::getMaxNumThreads() );
+#else
+        m_solver = new btSequentialImpulseConstraintSolver();
+#endif //#if USE_PARALLEL_ISLAND_SOLVER
+        btDiscreteDynamicsWorld* world = new MyDiscreteDynamicsWorld( m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration );
+        m_dynamicsWorld = world;
+
+#if USE_PARALLEL_ISLAND_SOLVER
+        if ( btSimulationIslandManagerMt* islandMgr = dynamic_cast<btSimulationIslandManagerMt*>( world->getSimulationIslandManager() ) )
+        {
+            islandMgr->setIslandDispatchFunction( parallelIslandDispatch );
+            m_multithreadedWorld = true;
+        }
+#endif //#if USE_PARALLEL_ISLAND_SOLVER
+    }
+    else
+    {
+        // single threaded world
+        m_multithreadedWorld = false;
+
+        ///collision configuration contains default setup for memory, collision setup
+        m_collisionConfiguration = new btDefaultCollisionConfiguration();
+        //m_collisionConfiguration->setConvexConvexMultipointIterations();
+
+        ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+        m_dispatcher = new	btCollisionDispatcher( m_collisionConfiguration );
+
+        m_broadphase = new btDbvtBroadphase();
+
+        ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
+        btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
+        m_solver = sol;
+
+        m_dynamicsWorld = new btDiscreteDynamicsWorld( m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration );
+    }
+    m_dynamicsWorld->setInternalTickCallback( profileBeginCallback, NULL, true );
+    m_dynamicsWorld->setInternalTickCallback( profileEndCallback, NULL, false );
+    m_dynamicsWorld->setGravity( btVector3( 0, -10, 0 ) );
+    createDefaultParameters();
+}
+
+
+void CommonRigidBodyBase::createDefaultParameters()
+{
+    if (m_multithreadCapable)
+    {
+        // create a button to toggle multithreaded world
+        ButtonParams button( "Multithreaded world enable", 0, true );
+        button.m_userPointer = &gMultithreadedWorld;
+        button.m_callback = boolPtrButtonCallback;
+        m_guiHelper->getParameterInterface()->registerButtonParameter( button );
+    }
+    {
+        // create a button to toggle profile printing
+        ButtonParams button( "Display profile timings", 0, true );
+        button.m_userPointer = &gDisplayProfileInfo;
+        button.m_callback = boolPtrButtonCallback;
+        m_guiHelper->getParameterInterface()->registerButtonParameter( button );
+    }
+    if (m_multithreadedWorld)
+    {
+        // create a button for each supported threading API
+        for (int iApi = 0; iApi < TaskManager::apiCount; ++iApi)
+        {
+            TaskManager::Api api = static_cast<TaskManager::Api>(iApi);
+            if (gTaskMgr.isSupported(api))
+            {
+                char str[1024];
+                sprintf(str, "API %s", gTaskMgr.getApiName(api));
+                ButtonParams button( str, iApi, false );
+                button.m_callback = apiSelectButtonCallback;
+                m_guiHelper->getParameterInterface()->registerButtonParameter( button );
+            }
+        }
+        {
+            // create a slider to set the number of threads to use
+            gSliderNumThreads = float(gTaskMgr.getNumThreads());
+			SliderParams slider("Thread count", &gSliderNumThreads);
+			slider.m_minVal = 1.0f;
+			slider.m_maxVal = float(gTaskMgr.getMaxNumThreads()*2);
+			slider.m_callback = setThreadCountCallback;
+			slider.m_clampToNotches = false;
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
+        }
+    }
+}
+
+void CommonRigidBodyBase::physicsDebugDraw(int debugFlags)
+{
+	if (m_dynamicsWorld && m_dynamicsWorld->getDebugDrawer())
+	{
+		m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugFlags);
+		m_dynamicsWorld->debugDrawWorld();
+	}
+    char msg[ 1024 ];
+    int xCoord = 400;
+    int yCoord = 30;
+    int yStep = 30;
+    if (m_multithreadCapable)
+    {
+        if ( m_multithreadedWorld != gMultithreadedWorld )
+        {
+            sprintf( msg, "restart example to begin in %s mode",
+                     gMultithreadedWorld ? "multithreaded" : "single threaded"
+                     );
+            m_guiHelper->getAppInterface()->drawText( msg, 300, yCoord, 0.4f );
+            yCoord += yStep;
+        }
+    }
+    if (gDisplayProfileInfo)
+    {
+        if ( m_multithreadedWorld )
+        {
+            int numManifolds = m_dispatcher->getNumManifolds();
+            int numContacts = 0;
+            for ( int i = 0; i < numManifolds; ++i )
+            {
+                const btPersistentManifold* man = m_dispatcher->getManifoldByIndexInternal( i );
+                numContacts += man->getNumContacts();
+            }
+            const char* mtApi = TaskManager::getApiName( gTaskMgr.getApi() );
+            sprintf( msg, "islands=%d bodies=%d manifolds=%d contacts=%d [%s] threads=%d",
+                     gNumIslands,
+                     m_dynamicsWorld->getNumCollisionObjects(),
+                     numManifolds,
+                     numContacts,
+                     mtApi,
+                     gTaskMgr.getApi() == TaskManager::apiNone ? 1 : gTaskMgr.getNumThreads()
+                     );
+            m_guiHelper->getAppInterface()->drawText( msg, 100, yCoord, 0.4f );
+            yCoord += yStep;
+        }
+
+        sprintf( msg, "internalSimStep %5.3f ms",
+                 gProfiler.getAverageTime( Profiler::kRecordInternalTimeStep )*0.001f
+                 );
+        m_guiHelper->getAppInterface()->drawText( msg, xCoord, yCoord, 0.4f );
+        yCoord += yStep;
+
+        if ( m_multithreadedWorld )
+        {
+            sprintf( msg,
+                     "DispatchCollisionPairs %5.3f ms",
+                     gProfiler.getAverageTime( Profiler::kRecordDispatchAllCollisionPairs )*0.001f
+                     );
+            m_guiHelper->getAppInterface()->drawText( msg, xCoord, yCoord, 0.4f );
+            yCoord += yStep;
+
+            sprintf( msg,
+                     "SolveAllIslands %5.3f ms",
+                     gProfiler.getAverageTime( Profiler::kRecordDispatchIslands )*0.001f
+                     );
+            m_guiHelper->getAppInterface()->drawText( msg, xCoord, yCoord, 0.4f );
+            yCoord += yStep;
+
+            sprintf( msg,
+                     "PredictUnconstrainedMotion %5.3f ms",
+                     gProfiler.getAverageTime( Profiler::kRecordPredictUnconstrainedMotion )*0.001f
+                     );
+            m_guiHelper->getAppInterface()->drawText( msg, xCoord, yCoord, 0.4f );
+            yCoord += yStep;
+
+            sprintf( msg,
+                     "CreatePredictiveContacts %5.3f ms",
+                     gProfiler.getAverageTime( Profiler::kRecordCreatePredictiveContacts )*0.001f
+                     );
+            m_guiHelper->getAppInterface()->drawText( msg, xCoord, yCoord, 0.4f );
+            yCoord += yStep;
+
+            sprintf( msg,
+                     "IntegrateTransforms %5.3f ms",
+                     gProfiler.getAverageTime( Profiler::kRecordIntegrateTransforms )*0.001f
+                     );
+            m_guiHelper->getAppInterface()->drawText( msg, xCoord, yCoord, 0.4f );
+            yCoord += yStep;
+        }
+    }
+}
+

examples/CommonInterfaces/CommonRigidBodyBase.h

@@ -21,6 +21,8 @@ struct CommonRigidBodyBase : public CommonExampleInterface
 	btConstraintSolver*	m_solver;
 	btDefaultCollisionConfiguration* m_collisionConfiguration;
 	btDiscreteDynamicsWorld* m_dynamicsWorld;
+    bool m_multithreadedWorld;
+    bool m_multithreadCapable;
 
 	//data for picking objects
 	class btRigidBody*	m_pickedBody;
@@ -31,20 +33,8 @@ struct CommonRigidBodyBase : public CommonExampleInterface
 	btScalar m_oldPickingDist;
 	struct GUIHelperInterface* m_guiHelper;
 
-	CommonRigidBodyBase(struct GUIHelperInterface* helper)
+	CommonRigidBodyBase(struct GUIHelperInterface* helper);
-	:m_broadphase(0),
+	virtual ~CommonRigidBodyBase();
-		m_dispatcher(0),
-		m_solver(0),
-		m_collisionConfiguration(0),
-		m_dynamicsWorld(0),
-		m_pickedBody(0),
-		m_pickedConstraint(0),
-		m_guiHelper(helper)
-	{
-	}
-	virtual ~CommonRigidBodyBase()
-	{
-	}
 
 
 	btDiscreteDynamicsWorld*	getDynamicsWorld()
@@ -52,26 +42,8 @@ struct CommonRigidBodyBase : public CommonExampleInterface
 		return m_dynamicsWorld;
 	}
 
-	virtual void createEmptyDynamicsWorld()
+    virtual void createDefaultParameters();
-	{
+	virtual void createEmptyDynamicsWorld();
-		///collision configuration contains default setup for memory, collision setup
-		m_collisionConfiguration = new btDefaultCollisionConfiguration();
-		//m_collisionConfiguration->setConvexConvexMultipointIterations();
-
-		///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
-		m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
-
-		m_broadphase = new btDbvtBroadphase();
-
-		///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
-		btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
-		m_solver = sol;
-
-		m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
-
-		m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
-	}
-
 
 	virtual void stepSimulation(float deltaTime)
 	{
@@ -81,14 +53,7 @@ struct CommonRigidBodyBase : public CommonExampleInterface
 		}
 	}
 
-	virtual void physicsDebugDraw(int debugFlags)
+	virtual void physicsDebugDraw(int debugFlags);
-	{
-		if (m_dynamicsWorld && m_dynamicsWorld->getDebugDrawer())
-		{
-			m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugFlags);
-			m_dynamicsWorld->debugDrawWorld();
-		}
-	}
 
 	virtual void exitPhysics()
 	{

examples/CommonInterfaces/ParallelFor.h

@@ -0,0 +1,336 @@
+/*
+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 <stdio.h> //printf debugging
+#include <algorithm>
+
+
+// choose threading providers:
+#if BT_USE_TBB
+#define USE_TBB 1     // use Intel Threading Building Blocks for thread management
+#endif
+
+#if BT_USE_PPL
+#define USE_PPL 1     // use Microsoft Parallel Patterns Library (installed with Visual Studio 2010 and later)
+#endif // BT_USE_PPL
+
+#if BT_USE_OPENMP
+#define USE_OPENMP 1  // use OpenMP (also need to change compiler options for OpenMP support)
+#endif
+
+
+#if USE_OPENMP
+
+#include <omp.h>
+
+#endif // #if USE_OPENMP
+
+
+#if USE_PPL
+
+#include <ppl.h>  // if you get a compile error here, check whether your version of Visual Studio includes PPL
+// Visual Studio 2010 and later should come with it
+#include <concrtrm.h>  // for GetProcessorCount()
+#endif // #if USE_PPL
+
+
+#if USE_TBB
+
+#define __TBB_NO_IMPLICIT_LINKAGE 1
+#include <tbb/tbb.h>
+#include <tbb/task_scheduler_init.h>
+#include <tbb/parallel_for.h>
+#include <tbb/blocked_range.h>
+
+#endif // #if USE_TBB
+
+
+
+class TaskManager
+{
+public:
+    enum Api
+    {
+        apiNone,
+        apiOpenMP,
+        apiTbb,
+        apiPpl,
+        apiCount
+    };
+    static const char* getApiName( Api api )
+    {
+        switch ( api )
+        {
+        case apiNone: return "None";
+        case apiOpenMP: return "OpenMP";
+        case apiTbb: return "Intel TBB";
+        case apiPpl: return "MS PPL";
+        default: return "unknown";
+        }
+    }
+
+    TaskManager()
+    {
+        m_api = apiNone;
+        m_numThreads = 0;
+#if USE_TBB
+        m_tbbSchedulerInit = NULL;
+#endif // #if USE_TBB
+    }
+
+    Api getApi() const
+    {
+        return m_api;
+    }
+
+    bool isSupported( Api api ) const
+    {
+#if USE_OPENMP
+        if ( api == apiOpenMP )
+        {
+            return true;
+        }
+#endif
+#if USE_TBB
+        if ( api == apiTbb )
+        {
+            return true;
+        }
+#endif
+#if USE_PPL
+        if ( api == apiPpl )
+        {
+            return true;
+        }
+#endif
+        // apiNone is always "supported"
+        return api == apiNone;
+    }
+
+    void setApi( Api api )
+    {
+        if (isSupported(api))
+        {
+            m_api = api;
+        }
+        else
+        {
+            // no compile time support for selected API, fallback to "none"
+            m_api = apiNone;
+        }
+    }
+
+    static int getMaxNumThreads()
+    {
+#if USE_OPENMP
+        return omp_get_max_threads();
+#elif USE_PPL
+        return concurrency::GetProcessorCount();
+#elif USE_TBB
+        return tbb::task_scheduler_init::default_num_threads();
+#endif
+        return 1;
+    }
+
+    int getNumThreads() const
+    {
+        return m_numThreads;
+    }
+
+    int setNumThreads( int numThreads )
+    {
+        m_numThreads = ( std::max )( 1, numThreads );
+
+#if USE_OPENMP
+        omp_set_num_threads( m_numThreads );
+#endif
+
+#if USE_PPL
+        {
+            using namespace concurrency;
+            if ( CurrentScheduler::Id() != -1 )
+            {
+                CurrentScheduler::Detach();
+            }
+            SchedulerPolicy policy;
+            policy.SetConcurrencyLimits( m_numThreads, m_numThreads );
+            CurrentScheduler::Create( policy );
+        }
+#endif
+
+#if USE_TBB
+        if ( m_tbbSchedulerInit )
+        {
+            delete m_tbbSchedulerInit;
+            m_tbbSchedulerInit = NULL;
+        }
+        m_tbbSchedulerInit = new tbb::task_scheduler_init( m_numThreads );
+#endif
+        return m_numThreads;
+    }
+
+    void init()
+    {
+        if (m_numThreads == 0)
+        {
+#if USE_PPL
+            setApi( apiPpl );
+#endif
+#if USE_TBB
+            setApi( apiTbb );
+#endif
+#if USE_OPENMP
+            setApi( apiOpenMP );
+#endif
+            setNumThreads(getMaxNumThreads());
+        }
+        else
+        {
+            setNumThreads(m_numThreads);
+        }
+    }
+
+    void shutdown()
+    {
+#if USE_TBB
+        if ( m_tbbSchedulerInit )
+        {
+            delete m_tbbSchedulerInit;
+            m_tbbSchedulerInit = NULL;
+        }
+#endif
+    }
+
+private:
+    Api m_api;
+    int m_numThreads;
+#if USE_TBB
+    tbb::task_scheduler_init* m_tbbSchedulerInit;
+#endif // #if USE_TBB
+};
+
+extern TaskManager gTaskMgr;
+
+
+static void initTaskScheduler()
+{
+    gTaskMgr.init();
+}
+
+static void cleanupTaskScheduler()
+{
+    gTaskMgr.shutdown();
+}
+
+
+#if USE_TBB
+///
+/// TbbBodyAdapter -- Converts a body object that implements the
+///                   "forLoop(int iBegin, int iEnd) const" function
+///  into a TBB compatible object that takes a tbb::blocked_range<int> type.
+///
+template <class TBody>
+struct TbbBodyAdapter
+{
+    const TBody* mBody;
+
+    void operator()( const tbb::blocked_range<int>& range ) const
+    {
+        mBody->forLoop( range.begin(), range.end() );
+    }
+};
+#endif // #if USE_TBB
+
+#if USE_PPL
+///
+/// PplBodyAdapter -- Converts a body object that implements the
+///                   "forLoop(int iBegin, int iEnd) const" function
+///  into a PPL compatible object that implements "void operator()( int ) const"
+///
+template <class TBody>
+struct PplBodyAdapter
+{
+    const TBody* mBody;
+    int mGrainSize;
+    int mIndexEnd;
+
+    void operator()( int i ) const
+    {
+        mBody->forLoop( i, (std::min)(i + mGrainSize, mIndexEnd) );
+    }
+};
+#endif // #if USE_PPL
+
+
+///
+/// parallelFor -- interface for submitting work expressed as a for loop to the worker threads
+///
+template <class TBody>
+void parallelFor( int iBegin, int iEnd, int grainSize, const TBody& body )
+{
+#if USE_OPENMP
+    if ( gTaskMgr.getApi() == TaskManager::apiOpenMP )
+    {
+#pragma omp parallel for schedule(static, 1)
+        for ( int i = iBegin; i < iEnd; i += grainSize )
+        {
+            body.forLoop( i, (std::min)( i + grainSize, iEnd ) );
+        }
+        return;
+    }
+#endif // #if USE_OPENMP
+
+#if USE_PPL
+    if ( gTaskMgr.getApi() == TaskManager::apiPpl )
+    {
+        // PPL dispatch
+        PplBodyAdapter<TBody> pplBody;
+        pplBody.mBody = &body;
+        pplBody.mGrainSize = grainSize;
+        pplBody.mIndexEnd = iEnd;
+        // note: MSVC 2010 doesn't support partitioner args, so avoid them
+        concurrency::parallel_for( iBegin,
+                                   iEnd,
+                                   grainSize,
+                                   pplBody
+                                   );
+        return;
+    }
+#endif //#if USE_PPL
+
+#if USE_TBB
+    if ( gTaskMgr.getApi() == TaskManager::apiTbb )
+    {
+        // TBB dispatch
+        TbbBodyAdapter<TBody> tbbBody;
+        tbbBody.mBody = &body;
+        tbb::parallel_for( tbb::blocked_range<int>( iBegin, iEnd, grainSize ),
+                           tbbBody,
+                           tbb::simple_partitioner()
+                           );
+        return;
+    }
+#endif // #if USE_TBB
+
+    {
+        // run on main thread
+        body.forLoop( iBegin, iEnd );
+    }
+
+}
+
+
+
+

examples/ExampleBrowser/CMakeLists.txt

@@ -32,6 +32,7 @@ ADD_LIBRARY(BulletExampleBrowserLib
 	GL_ShapeDrawer.cpp
 	CollisionShape2TriangleMesh.cpp
 	CollisionShape2TriangleMesh.h
+	../CommonInterfaces/CommonRigidBodyBase.cpp
 	../Utils/b3Clock.cpp
 	../Utils/b3Clock.h
 	../Utils/b3ResourcePath.cpp
@@ -108,6 +109,29 @@ ELSE(WIN32)
 	ENDIF(APPLE)
 ENDIF(WIN32)
 
+IF (BULLET2_MULTITHREADED_OPEN_MP_DEMO)
+    ADD_DEFINITIONS("-DBT_USE_OPENMP=1")
+    IF (MSVC)
+        SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp")
+    ELSE (MSVC)
+        # GCC, Clang
+        SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
+    ENDIF (MSVC)
+ENDIF (BULLET2_MULTITHREADED_OPEN_MP_DEMO)
+
+IF (BULLET2_MULTITHREADED_PPL_DEMO)
+    ADD_DEFINITIONS("-DBT_USE_PPL=1")
+ENDIF (BULLET2_MULTITHREADED_PPL_DEMO)
+
+IF (BULLET2_MULTITHREADED_TBB_DEMO)
+    SET (BULLET2_TBB_INCLUDE_DIR "not found" CACHE PATH "Directory for Intel TBB includes.")
+    SET (BULLET2_TBB_LIB_DIR "not found" CACHE PATH "Directory for Intel TBB libraries.")
+    find_library(TBB_LIBRARY tbb PATHS ${BULLET2_TBB_LIB_DIR})
+    find_library(TBBMALLOC_LIBRARY tbbmalloc PATHS ${BULLET2_TBB_LIB_DIR})
+    ADD_DEFINITIONS("-DBT_USE_TBB=1")
+    INCLUDE_DIRECTORIES( ${BULLET2_TBB_INCLUDE_DIR} )
+    LINK_LIBRARIES( ${TBB_LIBRARY} ${TBBMALLOC_LIBRARY} )
+ENDIF (BULLET2_MULTITHREADED_TBB_DEMO)
 
 SET(ExtendedTutorialsSources
 	../ExtendedTutorials/Chain.cpp 
@@ -173,6 +197,8 @@ SET(BulletExampleBrowser_SRCS
 	../InverseKinematics/InverseKinematicsExample.h
 	../ForkLift/ForkLiftDemo.cpp
 	../ForkLift/ForkLiftDemo.h
+	../MultiThreadedDemo/MultiThreadedDemo.cpp
+	../MultiThreadedDemo/MultiThreadedDemo.h
 	../Tutorial/Tutorial.cpp
 	../Tutorial/Tutorial.h
 	../Tutorial/Dof6ConstraintTutorial.cpp
@@ -346,9 +372,26 @@ ADD_CUSTOM_COMMAND(
                 COMMAND ${CMAKE_COMMAND} ARGS -E copy_directory ${BULLET_PHYSICS_SOURCE_DIR}/data ${PROJECT_BINARY_DIR}/data
         )
 
+IF (BULLET2_MULTITHREADED_TBB_DEMO AND WIN32)
+    # add a post build command to copy some dlls to the executable directory
+    set(TBB_VC_VER "vc12")
+    set(TBB_VC_ARCH "ia32")
+    # assume 32-bit build in VC12 for now
+    # checks can be added here at a later time
+    ADD_CUSTOM_COMMAND(TARGET App_ExampleBrowser POST_BUILD
+    COMMAND ${CMAKE_COMMAND} -E copy_if_different
+        "${BULLET2_TBB_INCLUDE_DIR}/../bin/${TBB_VC_ARCH}/${TBB_VC_VER}/tbb.dll"
+        $<TARGET_FILE_DIR:App_ExampleBrowser>)
+    ADD_CUSTOM_COMMAND(TARGET App_ExampleBrowser POST_BUILD
+    COMMAND ${CMAKE_COMMAND} -E copy_if_different
+        "${BULLET2_TBB_INCLUDE_DIR}/../bin/${TBB_VC_ARCH}/${TBB_VC_VER}/tbbmalloc.dll"
+        $<TARGET_FILE_DIR:App_ExampleBrowser>)
+ENDIF (BULLET2_MULTITHREADED_TBB_DEMO AND WIN32)
+
 
 IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
 			SET_TARGET_PROPERTIES(App_ExampleBrowser PROPERTIES  DEBUG_POSTFIX "_Debug")
 			SET_TARGET_PROPERTIES(App_ExampleBrowser PROPERTIES  MINSIZEREL_POSTFIX "_MinsizeRel")
 			SET_TARGET_PROPERTIES(App_ExampleBrowser PROPERTIES  RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
 ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+

examples/ExampleBrowser/ExampleEntries.cpp

@@ -8,6 +8,7 @@
 #include "../RenderingExamples/TinyRendererSetup.h"
 #include "../RenderingExamples/DynamicTexturedCubeDemo.h"
 #include "../ForkLift/ForkLiftDemo.h"
+#include "../MultiThreadedDemo/MultiThreadedDemo.h"
 #include "../BasicDemo/BasicExample.h"
 #include "../Planar2D/Planar2D.h"
 #include "../Benchmarks/BenchmarkDemo.h"
@@ -284,7 +285,13 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"Fracture demo", "Create a basic custom implementation to model fracturing objects, based on a btCompoundShape. It explicitly propagates the collision impulses and breaks the rigid body into multiple rigid bodies. Press F to toggle fracture and glue mode.", FractureDemoCreateFunc),
 
 	ExampleEntry(1,"Planar 2D","Show the use of 2D collision shapes and rigid body simulation. The collision shape is wrapped into a btConvex2dShape. The rigid bodies are restricted in a plane using the 'setAngularFactor' and 'setLinearFactor' API call.",Planar2DCreateFunc),
-
+#if BT_USE_OPENMP || BT_USE_TBB || BT_USE_PPL
+    // only enable MultiThreaded demo if a task scheduler is available
+    ExampleEntry( 1, "Multithreaded Demo",
+    "Stacks of boxes that do not sleep. Good for testing performance with large numbers of bodies and contacts. Sliders can be used to change the number of stacks (restart needed after each change)."
+    ,
+    MultiThreadedDemoCreateFunc ),
+#endif
 
 
 	ExampleEntry(0,"Rendering"),

examples/ExampleBrowser/premake4.lua

@@ -95,6 +95,7 @@ project "App_BulletExampleBrowser"
 		"../RoboticsLearning/*",
 		"../Collision/Internal/*",
 		"../Benchmarks/*",
+		"../MultiThreadedDemo/*",
 		"../CommonInterfaces/*",
 		"../ForkLift/ForkLiftDemo.*",
 		"../Importers/**",

examples/ExtendedTutorials/premake4.lua

@@ -17,6 +17,7 @@ language "C++"
 
 files {
 	"RigidBodyFromObj.cpp",
+	"../CommonInterfaces/*",
 	"**.h",
 	"../StandaloneMain/main_console_single_example.cpp",
 		"../Utils/b3ResourcePath.cpp",
@@ -68,6 +69,7 @@ files {
         "RigidBodyFromObj.cpp",
         "*.h",
         "../StandaloneMain/main_opengl_single_example.cpp",
+	"../CommonInterfaces/*",
 	"../ExampleBrowser/OpenGLGuiHelper.cpp",
 	"../ExampleBrowser/GL_ShapeDrawer.cpp",
 	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
@@ -132,6 +134,7 @@ files {
 	"../ExampleBrowser/OpenGLGuiHelper.cpp",
 	"../ExampleBrowser/GL_ShapeDrawer.cpp",
 	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+	"../CommonInterfaces/*",
 	"../TinyRenderer/geometry.cpp",
 	"../TinyRenderer/model.cpp",
 	"../TinyRenderer/tgaimage.cpp",
@@ -193,6 +196,7 @@ files {
         "../StandaloneMain/main_tinyrenderer_single_example.cpp",
 	"../OpenGLWindow/SimpleCamera.cpp",
 	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+	"../CommonInterfaces/*",
 	"../TinyRenderer/geometry.cpp",
 	"../TinyRenderer/model.cpp",
 	"../TinyRenderer/tgaimage.cpp",

examples/MultiThreadedDemo/MultiThreadedDemo.cpp

@@ -0,0 +1,229 @@
+/*
+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 "btBulletDynamicsCommon.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#include <stdio.h> //printf debugging
+#include <algorithm>
+
+class btCollisionShape;
+
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonWindowInterface.h"
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "MultiThreadedDemo.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btBulletCollisionCommon.h"
+
+
+#define BT_OVERRIDE
+
+/// MultiThreadedDemo shows how to setup and use multithreading
+class MultiThreadedDemo  : public CommonRigidBodyBase
+{
+    static const int kUpAxis = 1;
+
+	btRigidBody* localCreateRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* colSape);
+
+    btVector3 m_cameraTargetPos;
+    float m_cameraPitch;
+    float m_cameraYaw;
+    float m_cameraDist;
+
+    void createStack( const btVector3& pos, btCollisionShape* boxShape, const btVector3& halfBoxSize, int size );
+    void createSceneObjects();
+    void destroySceneObjects();
+
+public:
+    BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    MultiThreadedDemo( struct GUIHelperInterface* helper );
+
+	virtual ~MultiThreadedDemo() {}
+
+    virtual void stepSimulation( float deltaTime ) BT_OVERRIDE
+    {
+        if ( m_dynamicsWorld )
+        {
+            // always step by 1/60 for benchmarking
+            m_dynamicsWorld->stepSimulation( 1.0f / 60.0f, 0 );
+        }
+    }
+
+    virtual void initPhysics() BT_OVERRIDE;
+    virtual void resetCamera() BT_OVERRIDE
+	{
+        m_guiHelper->resetCamera( m_cameraDist,
+                                  m_cameraPitch,
+                                  m_cameraYaw,
+                                  m_cameraTargetPos.x(),
+                                  m_cameraTargetPos.y(),
+                                  m_cameraTargetPos.z()
+                                  );
+	}
+
+};
+
+
+MultiThreadedDemo::MultiThreadedDemo(struct GUIHelperInterface* helper)
+    : CommonRigidBodyBase( helper )
+{
+    m_cameraTargetPos = btVector3( 0.0f, 0.0f, 0.0f );
+    m_cameraPitch = 90.0f;
+    m_cameraYaw = 30.0f;
+    m_cameraDist = 48.0f;
+    helper->setUpAxis( kUpAxis );
+}
+
+
+static btScalar gSliderStackRows = 8.0f;
+static btScalar gSliderStackColumns = 6.0f;
+
+
+void MultiThreadedDemo::initPhysics()
+{
+    createEmptyDynamicsWorld();
+
+    m_dynamicsWorld->setGravity( btVector3( 0, -10, 0 ) );
+
+    {
+        SliderParams slider( "Stack rows", &gSliderStackRows );
+        slider.m_minVal = 1.0f;
+        slider.m_maxVal = 20.0f;
+        slider.m_clampToNotches = false;
+        m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
+    }
+    {
+        SliderParams slider( "Stack columns", &gSliderStackColumns );
+        slider.m_minVal = 1.0f;
+        slider.m_maxVal = 20.0f;
+        slider.m_clampToNotches = false;
+        m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
+    }
+	
+    createSceneObjects();
+
+    m_guiHelper->createPhysicsDebugDrawer( m_dynamicsWorld );
+}
+
+
+
+btRigidBody* MultiThreadedDemo::localCreateRigidBody(btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
+{
+    btRigidBody* body = createRigidBody(mass, startTransform, shape);
+    if ( mass > 0.0f )
+    {
+        // prevent bodies from sleeping to make profiling/benchmarking easier
+        body->forceActivationState( DISABLE_DEACTIVATION );
+    }
+    return body;
+}
+
+
+void MultiThreadedDemo::createStack( const btVector3& center, btCollisionShape* boxShape, const btVector3& halfBoxSize, int size )
+{
+    btTransform trans;
+    trans.setIdentity();
+    float halfBoxHeight = halfBoxSize.y();
+    float halfBoxWidth = halfBoxSize.x();
+
+    for ( int i = 0; i<size; i++ )
+    {
+        // This constructs a row, from left to right
+        int rowSize = size - i;
+        for ( int j = 0; j< rowSize; j++ )
+        {
+            btVector3 pos = center + btVector3( halfBoxWidth*( 1 + j * 2 - rowSize ),
+                halfBoxHeight * ( 1 + i * 2),
+                0.0f
+                );
+
+            trans.setOrigin( pos );
+            btScalar mass = 1.f;
+
+            btRigidBody* body = localCreateRigidBody( mass, trans, boxShape );
+        }
+    }
+}
+
+
+void MultiThreadedDemo::createSceneObjects()
+{
+    {
+        // create ground box
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin( btVector3( 0, -3, 0 ) );
+
+        //either use heightfield or triangle mesh
+
+        btVector3 groundExtents( 400, 400, 400 );
+        groundExtents[ kUpAxis ] = 3;
+        btCollisionShape* groundShape = new btBoxShape( groundExtents );
+        m_collisionShapes.push_back( groundShape );
+
+        //create ground object
+        localCreateRigidBody( 0, tr, groundShape );
+    }
+
+    {
+        // create walls of cubes
+        const btVector3 halfExtents = btVector3( 0.5f, 0.25f, 0.5f );
+        int numStackRows = btMax(1, int(gSliderStackRows));
+        int numStackCols = btMax(1, int(gSliderStackColumns));
+        int stackHeight = 15;
+        float stackZSpacing = 3.0f;
+        float stackXSpacing = 20.0f;
+
+        btBoxShape* boxShape = new btBoxShape( halfExtents );
+        m_collisionShapes.push_back( boxShape );
+
+        for ( int iX = 0; iX < numStackCols; ++iX )
+        {
+            for ( int iZ = 0; iZ < numStackRows; ++iZ )
+            {
+                btVector3 center = btVector3( iX * stackXSpacing, 0.0f, ( iZ - numStackRows / 2 ) * stackZSpacing );
+                createStack( center, boxShape, halfExtents, stackHeight );
+            }
+        }
+    }
+
+    if ( false )
+    {
+        // destroyer ball
+        btTransform sphereTrans;
+        sphereTrans.setIdentity();
+        sphereTrans.setOrigin( btVector3( 0, 2, 40 ) );
+        btSphereShape* ball = new btSphereShape( 2.f );
+        m_collisionShapes.push_back( ball );
+        btRigidBody* ballBody = localCreateRigidBody( 10000.f, sphereTrans, ball );
+        ballBody->setLinearVelocity( btVector3( 0, 0, -10 ) );
+    }
+    m_guiHelper->autogenerateGraphicsObjects( m_dynamicsWorld );
+
+}
+
+
+CommonExampleInterface*    MultiThreadedDemoCreateFunc( struct CommonExampleOptions& options )
+{
+	return new MultiThreadedDemo(options.m_guiHelper);
+}
+

examples/MultiThreadedDemo/MultiThreadedDemo.h

@@ -0,0 +1,22 @@
+/*
+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.
+*/
+#ifndef MULTITHREADED_DEMO_H
+#define MULTITHREADED_DEMO_H
+
+class CommonExampleInterface*    MultiThreadedDemoCreateFunc(struct CommonExampleOptions& options);
+
+#endif // MULTITHREADED_DEMO_H
+
+

examples/OpenGLWindow/SimpleOpenGL2App.cpp

@@ -280,7 +280,7 @@ void SimpleOpenGL2App::swapBuffer()
 	m_window->startRendering();
 
 }
-void SimpleOpenGL2App::drawText( const char* txt, int posX, int posY)
+void SimpleOpenGL2App::drawText( const char* txt, int posX, int posY, float size)
 {
 
 }

examples/OpenGLWindow/SimpleOpenGL2App.h

@@ -17,7 +17,7 @@ public:
 	virtual int getUpAxis() const;
 	
 	virtual void swapBuffer();
-	virtual void drawText( const char* txt, int posX, int posY);
+	virtual void drawText( const char* txt, int posX, int posY, float size);
 	virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA){};
 	virtual void setBackgroundColor(float red, float green, float blue);
 	virtual int	registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ, int textureIndex = -1,  float textureScaling = 1)

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -352,7 +352,7 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world
 }
 
 
-void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi)
+void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi, float size)
 {
 
 	float posX = (float)posXi;
@@ -374,7 +374,7 @@ void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi)
 	{
 		bool measureOnly = false;
 
-		float fontSize= 64;//512;//128;
+		float fontSize= 64*size;//512;//128;
 		sth_draw_text(m_data->m_fontStash,
                     m_data->m_droidRegular,fontSize,posX,posY,
 					txt,&dx, this->m_instancingRenderer->getScreenWidth(),

examples/OpenGLWindow/SimpleOpenGL3App.h

@@ -31,7 +31,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
 	virtual int getUpAxis() const;
 	
 	virtual void swapBuffer();
-	virtual void drawText( const char* txt, int posX, int posY);
+	virtual void drawText( const char* txt, int posX, int posY, float size=1.0f);
 	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size);
 	virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA);
 	struct sth_stash* getFontStash();

src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -267,7 +267,6 @@ struct	btDbvt
 
 	
 	btAlignedObjectArray<sStkNN>	m_stkStack;
-	mutable btAlignedObjectArray<const btDbvtNode*>	m_rayTestStack;
 
 
 	// Methods
@@ -357,6 +356,7 @@ struct	btDbvt
 								btScalar lambda_max,
 								const btVector3& aabbMin,
 								const btVector3& aabbMax,
+                                btAlignedObjectArray<const btDbvtNode*>& stack,
 								DBVT_IPOLICY) const;
 
 	DBVT_PREFIX
@@ -1006,7 +1006,8 @@ inline void		btDbvt::rayTestInternal(	const btDbvtNode* root,
 								btScalar lambda_max,
 								const btVector3& aabbMin,
 								const btVector3& aabbMax,
-								DBVT_IPOLICY) const
+                                btAlignedObjectArray<const btDbvtNode*>& stack,
+                                DBVT_IPOLICY ) const
 {
         (void) rayTo;
 	DBVT_CHECKTYPE
@@ -1016,7 +1017,6 @@ inline void		btDbvt::rayTestInternal(	const btDbvtNode* root,
 
 		int								depth=1;
 		int								treshold=DOUBLE_STACKSIZE-2;
-		btAlignedObjectArray<const btDbvtNode*>&	stack = m_rayTestStack;
 		stack.resize(DOUBLE_STACKSIZE);
 		stack[0]=root;
 		btVector3 bounds[2];

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -16,6 +16,7 @@ subject to the following restrictions:
 ///btDbvtBroadphase implementation by Nathanael Presson
 
 #include "btDbvtBroadphase.h"
+#include "LinearMath/btThreads.h"
 
 //
 // Profiling
@@ -142,6 +143,11 @@ btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 	{
 		m_stageRoots[i]=0;
 	}
+#if BT_THREADSAFE
+    m_rayTestStacks.resize(BT_MAX_THREAD_COUNT);
+#else
+    m_rayTestStacks.resize(1);
+#endif
 #if DBVT_BP_PROFILE
 	clear(m_profiling);
 #endif
@@ -227,6 +233,23 @@ struct	BroadphaseRayTester : btDbvt::ICollide
 void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
 {
 	BroadphaseRayTester callback(rayCallback);
+    btAlignedObjectArray<const btDbvtNode*>* stack = &m_rayTestStacks[0];
+#if BT_THREADSAFE
+    // for this function to be threadsafe, each thread must have a separate copy
+    // of this stack.  This could be thread-local static to avoid dynamic allocations,
+    // instead of just a local.
+    int threadIndex = btGetCurrentThreadIndex();
+    btAlignedObjectArray<const btDbvtNode*> localStack;
+    if (threadIndex < m_rayTestStacks.size())
+    {
+        // use per-thread preallocated stack if possible to avoid dynamic allocations
+        stack = &m_rayTestStacks[threadIndex];
+    }
+    else
+    {
+        stack = &localStack;
+    }
+#endif
 
 	m_sets[0].rayTestInternal(	m_sets[0].m_root,
 		rayFrom,
@@ -236,6 +259,7 @@ void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
 		rayCallback.m_lambda_max,
 		aabbMin,
 		aabbMax,
+        *stack,
 		callback);
 
 	m_sets[1].rayTestInternal(	m_sets[1].m_root,
@@ -246,6 +270,7 @@ void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
 		rayCallback.m_lambda_max,
 		aabbMin,
 		aabbMax,
+        *stack,
 		callback);
 
 }

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -87,6 +87,7 @@ struct	btDbvtBroadphase : btBroadphaseInterface
 	bool					m_releasepaircache;			// Release pair cache on delete
 	bool					m_deferedcollide;			// Defere dynamic/static collision to collide call
 	bool					m_needcleanup;				// Need to run cleanup?
+    btAlignedObjectArray< btAlignedObjectArray<const btDbvtNode*> > m_rayTestStacks;
 #if DBVT_BP_PROFILE
 	btClock					m_clock;
 	struct	{

src/BulletCollision/CMakeLists.txt

@@ -33,6 +33,7 @@ SET(BulletCollision_SRCS
 	CollisionDispatch/btInternalEdgeUtility.h
 	CollisionDispatch/btManifoldResult.cpp
 	CollisionDispatch/btSimulationIslandManager.cpp
+	CollisionDispatch/btSimulationIslandManagerMt.cpp
 	CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
 	CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
 	CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
@@ -140,6 +141,7 @@ SET(CollisionDispatch_HDRS
 	CollisionDispatch/btHashedSimplePairCache.h
 	CollisionDispatch/btManifoldResult.h
 	CollisionDispatch/btSimulationIslandManager.h
+	CollisionDispatch/btSimulationIslandManagerMt.h
 	CollisionDispatch/btSphereBoxCollisionAlgorithm.h
 	CollisionDispatch/btSphereSphereCollisionAlgorithm.h
 	CollisionDispatch/btSphereTriangleCollisionAlgorithm.h

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -84,12 +84,8 @@ btPersistentManifold*	btCollisionDispatcher::getNewManifold(const btCollisionObj
 
 	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
 		
- 	void* mem = 0;
+ 	void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) );
-	
+    if (NULL == mem)
-	if (m_persistentManifoldPoolAllocator->getFreeCount())
-	{
-		mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
-	} else
 	{
         //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)
@@ -294,13 +290,13 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair,
 
 void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
 {
-	if (m_collisionAlgorithmPoolAllocator->getFreeCount())
+    void* mem = m_collisionAlgorithmPoolAllocator->allocate( size );
+    if (NULL == mem)
     {
-		return m_collisionAlgorithmPoolAllocator->allocate(size);
-	}
-	
 	    //warn user for overflow?
 	    return btAlignedAlloc(static_cast<size_t>(size), 16);
+    }
+    return mem;
 }
 
 void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -79,6 +79,7 @@ protected:
 
 	int				m_islandTag1;
 	int				m_companionId;
+    int             m_uniqueId;
 
 	mutable int				m_activationState1;
 	mutable btScalar			m_deactivationTime;
@@ -455,6 +456,16 @@ public:
 		m_companionId = id;
 	}
 
+    SIMD_FORCE_INLINE int getUniqueId() const
+    {
+        return	m_uniqueId;
+    }
+
+    void	setUniqueId( int id )
+    {
+        m_uniqueId = id;
+    }
+
     SIMD_FORCE_INLINE btScalar			getHitFraction() const
 	{
 		return m_hitFraction; 

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -179,11 +179,10 @@ static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
 
 
 
-btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver)
 {
 	m_numPerturbationIterations = 0;
 	m_minimumPointsPerturbationThreshold = 3;
-	m_simplexSolver = simplexSolver;
 	m_pdSolver = pdSolver;
 }
 
@@ -191,9 +190,8 @@ btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
 { 
 }
 
-btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_simplexSolver(simplexSolver),
 m_pdSolver(pdSolver),
 m_ownManifold (false),
 m_manifoldPtr(mf),
@@ -349,8 +347,8 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 
 	
 	btGjkPairDetector::ClosestPointInput input;
-
+    btVoronoiSimplexSolver simplexSolver;
-	btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+    btGjkPairDetector	gjkPairDetector( min0, min1, &simplexSolver, m_pdSolver );
 	//TODO: if (dispatchInfo.m_useContinuous)
 	gjkPairDetector.setMinkowskiA(min0);
 	gjkPairDetector.setMinkowskiB(min1);

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -43,7 +43,6 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
 #ifdef USE_SEPDISTANCE_UTIL2
 	btConvexSeparatingDistanceUtil	m_sepDistance;
 #endif
-	btSimplexSolverInterface*		m_simplexSolver;
 	btConvexPenetrationDepthSolver* m_pdSolver;
 
 	btVertexArray worldVertsB1;
@@ -62,7 +61,7 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
 
 public:
 
-	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
 
 	virtual ~btConvexConvexAlgorithm();
 
@@ -90,18 +89,17 @@ public:
 	{
 
 		btConvexPenetrationDepthSolver*		m_pdSolver;
-		btSimplexSolverInterface*			m_simplexSolver;
 		int m_numPerturbationIterations;
 		int m_minimumPointsPerturbationThreshold;
 
-		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+		CreateFunc(btConvexPenetrationDepthSolver* pdSolver);
 		
 		virtual ~CreateFunc();
 
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
-			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
 		}
 	};
 

src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -44,9 +44,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 //btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
 {
 
-	void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16);
+    void* mem = NULL;
-	m_simplexSolver = new (mem)btVoronoiSimplexSolver();
-
 	if (constructionInfo.m_useEpaPenetrationAlgorithm)
 	{
 		mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
@@ -59,7 +57,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 	
 	//default CreationFunctions, filling the m_doubleDispatch table
 	mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
-	m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
+	m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver);
 	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
 	m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc;
 	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
@@ -193,9 +191,6 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 	m_planeConvexCF->~btCollisionAlgorithmCreateFunc();
 	btAlignedFree( m_planeConvexCF);
 
-	m_simplexSolver->~btVoronoiSimplexSolver();
-	btAlignedFree(m_simplexSolver);
-
 	m_pdSolver->~btConvexPenetrationDepthSolver();
 	
 	btAlignedFree(m_pdSolver);

src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

@@ -60,8 +60,7 @@ protected:
 	btPoolAllocator*	m_collisionAlgorithmPool;
 	bool	m_ownsCollisionAlgorithmPool;
 
-	//default simplex/penetration depth solvers
+	//default penetration depth solver
-	btVoronoiSimplexSolver*	m_simplexSolver;
 	btConvexPenetrationDepthSolver*	m_pdSolver;
 	
 	//default CreationFunctions, filling the m_doubleDispatch table
@@ -102,12 +101,6 @@ public:
 	}
 
 
-	virtual	btVoronoiSimplexSolver*	getSimplexSolver()
-	{
-		return m_simplexSolver;
-	}
-
-
 	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
 
 	///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.

src/BulletCollision/CollisionDispatch/btSimulationIslandManagerMt.cpp

@@ -0,0 +1,641 @@
+/*
+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 "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 <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 = defaultIslandDispatch;
+    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	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;
+}
+
+/// 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 )
+{
+    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("islandUnionFindAndQuickSort");
+	
+	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)
+				{
+					allSleeping = false;
+				}
+				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+				}
+			}
+		}
+
+		if (allSleeping)
+		{
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					colObj0->setActivationState( ISLAND_SLEEPING );
+				}
+			}
+		} else
+		{
+
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+					{
+						colObj0->setActivationState( WANTS_DEACTIVATION);
+						colObj0->setDeactivationTime(0.f);
+					}
+				}
+			}
+		}
+	}
+}
+
+
+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 = 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++;
+    }
+}
+
+
+void btSimulationIslandManagerMt::defaultIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+{
+    // serial dispatch
+    btAlignedObjectArray<Island*>& islands = *islandsPtr;
+    for ( int i = 0; i < islands.size(); ++i )
+    {
+        Island* island = islands[ i ];
+        btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
+        btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
+        callback->processIsland( &island->bodyArray[ 0 ],
+                                 island->bodyArray.size(),
+                                 manifolds,
+                                 island->manifoldArray.size(),
+                                 constraintsPtr,
+                                 island->constraintArray.size(),
+                                 island->id
+                                 );
+    }
+}
+
+///@todo: this is random access, it can be walked 'cache friendly'!
+void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher,
+                                                        btCollisionWorld* collisionWorld,
+                                                        btAlignedObjectArray<btTypedConstraint*>& constraints,
+                                                        IslandCallback* callback
+                                                        )
+{
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	buildIslands(dispatcher,collisionWorld);
+
+	BT_PROFILE("processIslands");
+
+	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;
+		callback->processIsland(&collisionObjects[0],
+                                 collisionObjects.size(),
+                                 manifolds,
+                                 maxNumManifolds,
+                                 constraintsPtr,
+                                 constraints.size(),
+                                 -1
+                                 );
+	}
+	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, callback );
+	}
+}

src/BulletCollision/CollisionDispatch/btSimulationIslandManagerMt.h

@@ -0,0 +1,109 @@
+/*
+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.
+*/
+
+#ifndef BT_SIMULATION_ISLAND_MANAGER_MT_H
+#define BT_SIMULATION_ISLAND_MANAGER_MT_H
+
+#include "btSimulationIslandManager.h"
+
+class btTypedConstraint;
+
+
+///
+/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
+///                       Splits the world up into islands which can be solved in parallel.
+///                       In order to solve islands in parallel, an IslandDispatch function
+///                       must be provided which will dispatch calls to multiple threads.
+///                       The amount of parallelism that can be achieved depends on the number
+///                       of islands. If only a single island exists, then no parallelism is
+///                       possible.
+///
+class btSimulationIslandManagerMt : public btSimulationIslandManager
+{
+public:
+    struct Island
+    {
+        // a simulation island consisting of bodies, manifolds and constraints,
+        // to be passed into a constraint solver.
+        btAlignedObjectArray<btCollisionObject*> bodyArray;
+        btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+        btAlignedObjectArray<btTypedConstraint*> constraintArray;
+        int id;  // island id
+        bool isSleeping;
+
+        void append( const Island& other );  // add bodies, manifolds, constraints to my own
+    };
+    struct	IslandCallback
+    {
+        virtual ~IslandCallback() {};
+
+        virtual	void processIsland( btCollisionObject** bodies,
+                                    int numBodies,
+                                    btPersistentManifold** manifolds,
+                                    int numManifolds,
+                                    btTypedConstraint** constraints,
+                                    int numConstraints,
+                                    int islandId
+                                    ) = 0;
+    };
+    typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
+    static void defaultIslandDispatch( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
+protected:
+    btAlignedObjectArray<Island*> m_allocatedIslands;  // owner of all Islands
+    btAlignedObjectArray<Island*> m_activeIslands;  // islands actively in use
+    btAlignedObjectArray<Island*> m_freeIslands;  // islands ready to be reused
+    btAlignedObjectArray<Island*> m_lookupIslandFromId;  // big lookup table to map islandId to Island pointer
+    Island* m_batchIsland;
+    int m_minimumSolverBatchSize;
+    int m_batchIslandMinBodyCount;
+    IslandDispatchFunc m_islandDispatch;
+
+    Island* getIsland( int id );
+    virtual Island* allocateIsland( int id, int numBodies );
+    virtual void initIslandPools();
+    virtual void addBodiesToIslands( btCollisionWorld* collisionWorld );
+    virtual void addManifoldsToIslands( btDispatcher* dispatcher );
+    virtual void addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints );
+    virtual void mergeIslands();
+	
+public:
+	btSimulationIslandManagerMt();
+	virtual ~btSimulationIslandManagerMt();
+
+    virtual void buildAndProcessIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld, btAlignedObjectArray<btTypedConstraint*>& constraints, IslandCallback* callback );
+
+	virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+
+    int getMinimumSolverBatchSize() const
+    {
+        return m_minimumSolverBatchSize;
+    }
+    void setMinimumSolverBatchSize( int sz )
+    {
+        m_minimumSolverBatchSize = sz;
+    }
+    IslandDispatchFunc getIslandDispatchFunction() const
+    {
+        return m_islandDispatch;
+    }
+    // allow users to set their own dispatch function for multithreaded dispatch
+    void setIslandDispatchFunction( IslandDispatchFunc func )
+    {
+        m_islandDispatch = func;
+    }
+};
+
+#endif //BT_SIMULATION_ISLAND_MANAGER_H
+

src/BulletCollision/Gimpact/btGImpactShape.cpp

@@ -23,6 +23,59 @@ subject to the following restrictions:
 #include "btGImpactMassUtil.h"
 
 
+btGImpactMeshShapePart::btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part )
+{
+    // moved from .h to .cpp because of conditional compilation
+    // (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to
+    // avoid using it in h files)
+    m_primitive_manager.m_meshInterface = meshInterface;
+    m_primitive_manager.m_part = part;
+    m_box_set.setPrimitiveManager( &m_primitive_manager );
+#if BT_THREADSAFE
+    // If threadsafe is requested, this object uses a different lock/unlock
+    //  model with the btStridingMeshInterface -- lock once when the object is constructed
+    //  and unlock once in the destructor.
+    // The other way of locking and unlocking for each collision check in the narrowphase
+    // is not threadsafe.  Note these are not thread-locks, they are calls to the meshInterface's
+    // getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of
+    // pointers.  In theory a client could override the lock function to do all sorts of
+    // things like reading data from GPU memory, or decompressing data on the fly, but such things
+    // do not seem all that likely or useful, given the performance cost.
+    m_primitive_manager.lock();
+#endif
+}
+
+btGImpactMeshShapePart::~btGImpactMeshShapePart()
+{
+    // moved from .h to .cpp because of conditional compilation
+#if BT_THREADSAFE
+    m_primitive_manager.unlock();
+#endif
+}
+
+void btGImpactMeshShapePart::lockChildShapes() const
+{
+    // moved from .h to .cpp because of conditional compilation
+#if ! BT_THREADSAFE
+    // called in the narrowphase -- not threadsafe!
+    void * dummy = (void*) ( m_box_set.getPrimitiveManager() );
+    TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>( dummy );
+    dummymanager->lock();
+#endif
+}
+
+void btGImpactMeshShapePart::unlockChildShapes()  const
+{
+    // moved from .h to .cpp because of conditional compilation
+#if ! BT_THREADSAFE
+    // called in the narrowphase -- not threadsafe!
+    void * dummy = (void*) ( m_box_set.getPrimitiveManager() );
+    TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>( dummy );
+    dummymanager->unlock();
+#endif
+}
+
+
 #define CALC_EXACT_INERTIA 1
 
 

src/BulletCollision/Gimpact/btGImpactShape.h

@@ -722,17 +722,8 @@ public:
 		m_box_set.setPrimitiveManager(&m_primitive_manager);
 	}
 
-
+    btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part );
-	btGImpactMeshShapePart(btStridingMeshInterface * meshInterface,	int part)
+    virtual ~btGImpactMeshShapePart();
-	{
-		m_primitive_manager.m_meshInterface = meshInterface;
-		m_primitive_manager.m_part = part;
-		m_box_set.setPrimitiveManager(&m_primitive_manager);
-	}
-
-	virtual ~btGImpactMeshShapePart()
-	{
-	}
 
 	//! if true, then its children must get transforms.
 	virtual bool childrenHasTransform() const
@@ -742,19 +733,8 @@ public:
 
 
 	//! call when reading child shapes
-	virtual void lockChildShapes() const
+    virtual void lockChildShapes() const;
-	{
+    virtual void unlockChildShapes()  const;
-		void * dummy = (void*)(m_box_set.getPrimitiveManager());
-		TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
-		dummymanager->lock();
-	}
-
-	virtual void unlockChildShapes()  const
-	{
-		void * dummy = (void*)(m_box_set.getPrimitiveManager());
-		TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
-		dummymanager->unlock();
-	}
 
 	//! Gets the number of children
 	virtual int	getNumChildShapes() const

src/BulletDynamics/CMakeLists.txt

@@ -21,6 +21,7 @@ SET(BulletDynamics_SRCS
 	ConstraintSolver/btTypedConstraint.cpp
 	ConstraintSolver/btUniversalConstraint.cpp
 	Dynamics/btDiscreteDynamicsWorld.cpp
+	Dynamics/btDiscreteDynamicsWorldMt.cpp
 	Dynamics/btRigidBody.cpp
 	Dynamics/btSimpleDynamicsWorld.cpp
 #	Dynamics/Bullet-C-API.cpp
@@ -70,6 +71,7 @@ SET(ConstraintSolver_HDRS
 SET(Dynamics_HDRS
 	Dynamics/btActionInterface.h
 	Dynamics/btDiscreteDynamicsWorld.h
+	Dynamics/btDiscreteDynamicsWorldMt.h
 	Dynamics/btDynamicsWorld.h
 	Dynamics/btSimpleDynamicsWorld.h
 	Dynamics/btRigidBody.h

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -716,6 +716,62 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addTorsionalFrictionCon
 
 int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
 {
+#if BT_THREADSAFE
+    int solverBodyId = -1;
+    if ( !body.isStaticOrKinematicObject() )
+    {
+        // dynamic body
+        // Dynamic bodies can only be in one island, so it's safe to write to the companionId
+        solverBodyId = body.getCompanionId();
+        if ( solverBodyId < 0 )
+        {
+            if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
+            {
+                solverBodyId = m_tmpSolverBodyPool.size();
+                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+                initSolverBody( &solverBody, &body, timeStep );
+                body.setCompanionId( solverBodyId );
+            }
+        }
+    }
+    else if ( body.isStaticObject() )
+    {
+        // all fixed bodies (inf mass) get mapped to a single solver id
+        if ( m_fixedBodyId < 0 )
+        {
+            m_fixedBodyId = m_tmpSolverBodyPool.size();
+            btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+            initSolverBody( &fixedBody, 0, timeStep );
+        }
+        solverBodyId = m_fixedBodyId;
+    }
+    else
+    {
+        // kinematic
+        // Kinematic bodies can be in multiple islands at once, so it is a
+        // race condition to write to them, so we use an alternate method
+        // to record the solverBodyId
+        int uniqueId = body.getUniqueId();
+        const int INVALID_SOLVER_BODY_ID = -1;
+        if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
+        {
+            m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+        }
+        solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ];
+        // if no table entry yet,
+        if ( solverBodyId == INVALID_SOLVER_BODY_ID )
+        {
+            // create a table entry for this body
+            btRigidBody* rb = btRigidBody::upcast( &body );
+            solverBodyId = m_tmpSolverBodyPool.size();
+            btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+            initSolverBody( &solverBody, &body, timeStep );
+            m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId;
+        }
+    }
+    btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
+	return solverBodyId;
+#else // BT_THREADSAFE
 
     int solverBodyIdA = -1;
 
@@ -749,6 +805,7 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 	}
 
 	return solverBodyIdA;
+#endif // BT_THREADSAFE
 
 }
 #include <stdio.h>
@@ -1263,7 +1320,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	{
 		bodies[i]->setCompanionId(-1);
 	}
-
+#if BT_THREADSAFE
+    m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 );
+#endif // BT_THREADSAFE
 
 	m_tmpSolverBodyPool.reserve(numBodies+1);
 	m_tmpSolverBodyPool.resize(0);

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -45,6 +45,14 @@ protected:
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
 	int							m_maxOverrideNumSolverIterations;
 	int m_fixedBodyId;
+    // When running solvers on multiple threads, a race condition exists for Kinematic objects that
+    // participate in more than one solver.
+    // The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body
+    // for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island
+    // (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once.
+    // To avoid this race condition, this solver does not write the companionId, instead it stores the solver body
+    // index in this solver-local table, indexed by the uniqueId of the body.
+    btAlignedObjectArray<int>	m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
 
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -878,25 +878,12 @@ public:
 int gNumClampedCcdMotions=0;
 
 
-void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
+void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep)
 {
-	BT_PROFILE("createPredictiveContacts");
-
-	{
-		BT_PROFILE("release predictive contact manifolds");
-
-		for (int i=0;i<m_predictiveManifolds.size();i++)
-		{
-			btPersistentManifold* manifold = m_predictiveManifolds[i];
-			this->m_dispatcher1->releaseManifold(manifold);
-		}
-		m_predictiveManifolds.clear();
-	}
-
 	btTransform predictedTrans;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for ( int i=0;i<numBodies;i++)
 	{
-		btRigidBody* body = m_nonStaticRigidBodies[i];
+		btRigidBody* body = bodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
@@ -953,7 +940,9 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 
 
 						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
+                        btMutexLock( &m_predictiveManifoldsMutex );
 						m_predictiveManifolds.push_back(manifold);
+                        btMutexUnlock( &m_predictiveManifoldsMutex );
 
 						btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
 						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
@@ -974,13 +963,35 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 		}
 	}
 }
-void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+
+void btDiscreteDynamicsWorld::releasePredictiveContacts()
 {
-	BT_PROFILE("integrateTransforms");
+    BT_PROFILE( "release predictive contact manifolds" );
-	btTransform predictedTrans;
+
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+    for ( int i = 0; i < m_predictiveManifolds.size(); i++ )
     {
-		btRigidBody* body = m_nonStaticRigidBodies[i];
+        btPersistentManifold* manifold = m_predictiveManifolds[ i ];
+        this->m_dispatcher1->releaseManifold( manifold );
+    }
+    m_predictiveManifolds.clear();
+}
+
+void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
+{
+	BT_PROFILE("createPredictiveContacts");
+    releasePredictiveContacts();
+    if (m_nonStaticRigidBodies.size() > 0)
+    {
+        createPredictiveContactsInternal( &m_nonStaticRigidBodies[ 0 ], m_nonStaticRigidBodies.size(), timeStep );
+    }
+}
+
+void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep )
+{
+	btTransform predictedTrans;
+	for (int i=0;i<numBodies;i++)
+	{
+		btRigidBody* body = bodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
@@ -1080,6 +1091,16 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 	}
 
+}
+
+void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	BT_PROFILE("integrateTransforms");
+    if (m_nonStaticRigidBodies.size() > 0)
+    {
+        integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
+    }
+
     ///this should probably be switched on by default, but it is not well tested yet
 	if (m_applySpeculativeContactRestitution)
 	{
@@ -1114,14 +1135,12 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			}
 		}
 	}
-
 }
 
 
 
 
 
-
 void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
 	BT_PROFILE("predictUnconstraintMotion");

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -30,6 +30,7 @@ class btIDebugDraw;
 struct InplaceSolverIslandCallback;
 
 #include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btThreads.h"
 
 
 ///btDiscreteDynamicsWorld provides discrete rigid body simulation
@@ -68,9 +69,11 @@ protected:
 	bool	m_latencyMotionStateInterpolation;
 
 	btAlignedObjectArray<btPersistentManifold*>	m_predictiveManifolds;
+    btSpinMutex m_predictiveManifoldsMutex;  // used to synchronize threads creating predictive contacts
 
 	virtual void	predictUnconstraintMotion(btScalar timeStep);
 	
+    void integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep );  // can be called in parallel
 	virtual void	integrateTransforms(btScalar timeStep);
 		
 	virtual void	calculateSimulationIslands();
@@ -85,7 +88,9 @@ protected:
 
 	virtual void	internalSingleStepSimulation( btScalar timeStep);
 
-	void	createPredictiveContacts(btScalar timeStep);
+    void releasePredictiveContacts();
+    void createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep );  // can be called in parallel
+	virtual void	createPredictiveContacts(btScalar timeStep);
 
 	virtual void	saveKinematicState(btScalar timeStep);
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp

@@ -0,0 +1,168 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+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 "btDiscreteDynamicsWorldMt.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManagerMt.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btQuickprof.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#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
+                              );
+    }
+
+};
+
+
+
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+	if (m_ownsIslandManager)
+	{
+		m_islandManager->~btSimulationIslandManager();
+		btAlignedFree( m_islandManager);
+	}
+    {
+		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16);
+		m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher);
+    }
+	{
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
+		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
+        m_islandManager = im;
+        im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
+	}
+}
+
+
+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 );
+
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+}
+
+
+void btDiscreteDynamicsWorldMt::addCollisionObject(btCollisionObject* collisionObject, short int collisionFilterGroup, short int collisionFilterMask)
+{
+    collisionObject->setUniqueId(m_collisionObjects.size());
+    btDiscreteDynamicsWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask);
+}

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h

@@ -0,0 +1,44 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+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.
+*/
+
+
+#ifndef BT_DISCRETE_DYNAMICS_WORLD_MT_H
+#define BT_DISCRETE_DYNAMICS_WORLD_MT_H
+
+#include "btDiscreteDynamicsWorld.h"
+
+struct InplaceSolverIslandCallbackMt;
+
+///
+/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
+///                              solving simulation islands on multiple threads.
+///
+ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
+{
+protected:
+    InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
+
+    virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	virtual ~btDiscreteDynamicsWorldMt();
+};
+
+#endif //BT_DISCRETE_DYNAMICS_WORLD_H

src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

@@ -215,12 +215,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
 	}
 
-	static btMatrixXu J3;
+    btMatrixXu& J3 = m_scratchJ3;
 	{
 		BT_PROFILE("J3.resize");
 		J3.resize(2*m,8);
 	}
-	static btMatrixXu JinvM3;
+    btMatrixXu& JinvM3 = m_scratchJInvM3;
 	{
 		BT_PROFILE("JinvM3.resize/setZero");
 
@@ -230,7 +230,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	}
 	int cur=0;
 	int rowOffset = 0;
-	static btAlignedObjectArray<int> ofs;
+    btAlignedObjectArray<int>& ofs = m_scratchOfs;
 	{
 		BT_PROFILE("ofs resize");
 		ofs.resize(0);
@@ -489,7 +489,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 		}
 	}
  
-	static btMatrixXu Minv;
+    btMatrixXu& Minv = m_scratchMInv;
 	Minv.resize(6*numBodies,6*numBodies);
 	Minv.setZero();
 	for (int i=0;i<numBodies;i++)
@@ -506,7 +506,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 				setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
 	}
  
-	static btMatrixXu J;
+    btMatrixXu& J = m_scratchJ;
 	J.resize(numConstraintRows,6*numBodies);
 	J.setZero();
  
@@ -541,10 +541,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 		}
 	}
  
-	static btMatrixXu J_transpose;
+    btMatrixXu& J_transpose = m_scratchJTranspose;
 	J_transpose= J.transpose();
 
-	static btMatrixXu tmp;
+    btMatrixXu& tmp = m_scratchTmp;
 
 	{
 		{

src/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -43,6 +43,17 @@ protected:
 	btMLCPSolverInterface* m_solver;
 	int m_fallback;
 
+    /// The following scratch variables are not stateful -- contents are cleared prior to each use.
+    /// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+    /// that multiple instances of the solver can be run in parallel.
+    btMatrixXu m_scratchJ3;
+    btMatrixXu m_scratchJInvM3;
+    btAlignedObjectArray<int> m_scratchOfs;
+    btMatrixXu m_scratchMInv;
+    btMatrixXu m_scratchJ;
+    btMatrixXu m_scratchJTranspose;
+    btMatrixXu m_scratchTmp;
+
 	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 

src/LinearMath/CMakeLists.txt

@@ -11,6 +11,7 @@ SET(LinearMath_SRCS
 	btPolarDecomposition.cpp
 	btQuickprof.cpp
 	btSerializer.cpp
+	btThreads.cpp
 	btVector3.cpp
 )
 
@@ -38,6 +39,7 @@ SET(LinearMath_HDRS
 	btScalar.h
 	btSerializer.h
 	btStackAlloc.h
+	btThreads.h
 	btTransform.h
 	btTransformUtil.h
 	btVector3.h

src/LinearMath/btAlignedObjectArray.h

@@ -322,7 +322,7 @@ protected:
 		{
 			public:
 
-				bool operator() ( const T& a, const T& b )
+				bool operator() ( const T& a, const T& b ) const
 				{
 					return ( a < b );
 				}

src/LinearMath/btConvexHull.cpp

@@ -87,7 +87,7 @@ btVector3  NormalOf(const btVector3 *vert, const int n);
 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
 {
 	// returns the point where the line p0-p1 intersects the plane n&d
-				static btVector3 dif;
+    btVector3 dif;
 		dif = p1-p0;
 				btScalar dn= btDot(plane.normal,dif);
 				btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
@@ -112,7 +112,7 @@ btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v
 
 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
 {
-	static btVector3 cp;
+	btVector3 cp;
 	cp = btCross(udir,vdir).normalized();
 
 	btScalar distu = -btDot(cp,ustart);

src/LinearMath/btPoolAllocator.h

@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "btScalar.h"
 #include "btAlignedAllocator.h"
+#include "btThreads.h"
 
 ///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
 class btPoolAllocator
@@ -27,6 +28,7 @@ class btPoolAllocator
 	int				m_freeCount;
 	void*			m_firstFree;
 	unsigned char*	m_pool;
+    btSpinMutex     m_mutex;  // only used if BT_THREADSAFE
 
 public:
 
@@ -71,11 +73,16 @@ public:
 	{
 		// release mode fix
 		(void)size;
+        btMutexLock(&m_mutex);
 		btAssert(!size || size<=m_elemSize);
-		btAssert(m_freeCount>0);
+		//btAssert(m_freeCount>0);  // should return null if all full
         void* result = m_firstFree;
+        if (NULL != m_firstFree)
+        {
             m_firstFree = *(void**)m_firstFree;
             --m_freeCount;
+        }
+        btMutexUnlock(&m_mutex);
         return result;
 	}
 
@@ -95,9 +102,11 @@ public:
 		 if (ptr) {
             btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
 
+            btMutexLock(&m_mutex);
             *(void**)ptr = m_firstFree;
             m_firstFree = ptr;
             ++m_freeCount;
+            btMutexUnlock(&m_mutex);
         }
 	}
 

src/LinearMath/btQuickprof.cpp

@@ -16,6 +16,9 @@
 #include "btQuickprof.h"
 
 
+#if BT_THREADSAFE
+#include "btThreads.h"
+#endif //#if BT_THREADSAFE
 
 
 #ifdef __CELLOS_LV2__
@@ -455,6 +458,14 @@ unsigned long int			CProfileManager::ResetTime = 0;
  *=============================================================================================*/
 void	CProfileManager::Start_Profile( const char * name )
 {
+#if BT_THREADSAFE
+    // profile system is not designed for profiling multiple threads
+    // disable collection on all but the main thread
+    if ( !btIsMainThread() )
+    {
+        return;
+    }
+#endif //#if BT_THREADSAFE
 	if (name != CurrentNode->Get_Name()) {
 		CurrentNode = CurrentNode->Get_Sub_Node( name );
 	}
@@ -468,6 +479,14 @@ void	CProfileManager::Start_Profile( const char * name )
  *=============================================================================================*/
 void	CProfileManager::Stop_Profile( void )
 {
+#if BT_THREADSAFE
+    // profile system is not designed for profiling multiple threads
+    // disable collection on all but the main thread
+    if ( !btIsMainThread() )
+    {
+        return;
+    }
+#endif //#if BT_THREADSAFE
 	// Return will indicate whether we should back up to our parent (we may
 	// be profiling a recursive function)
 	if (CurrentNode->Return()) {

src/LinearMath/btThreads.cpp

@@ -0,0 +1,230 @@
+/*
+Copyright (c) 2003-2014 Erwin Coumans  http://bullet.googlecode.com
+
+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 "btThreads.h"
+
+//
+// Lightweight spin-mutex based on atomics
+// Using ordinary system-provided mutexes like Windows critical sections was noticeably slower
+// presumably because when it fails to lock at first it would sleep the thread and trigger costly
+// context switching.
+// 
+
+#if BT_THREADSAFE
+
+#if __cplusplus >= 201103L
+
+// for anything claiming full C++11 compliance, use C++11 atomics
+// on GCC or Clang you need to compile with -std=c++11
+#define USE_CPP11_ATOMICS 1
+
+#elif defined( _MSC_VER )
+
+// on MSVC, use intrinsics instead
+#define USE_MSVC_INTRINSICS 1
+
+#elif defined( __GNUC__ ) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7))
+
+// available since GCC 4.7 and some versions of clang
+// todo: check for clang
+#define USE_GCC_BUILTIN_ATOMICS 1
+
+#elif defined( __GNUC__ ) && (__GNUC__ == 4 && __GNUC_MINOR__ >= 1)
+
+// available since GCC 4.1
+#define USE_GCC_BUILTIN_ATOMICS_OLD 1
+
+#endif
+
+
+#if USE_CPP11_ATOMICS
+
+#include <atomic>
+#include <thread>
+
+#define THREAD_LOCAL_STATIC thread_local static
+
+bool btSpinMutex::tryLock()
+{
+    std::atomic<int>* aDest = reinterpret_cast<std::atomic<int>*>(&mLock);
+    int expected = 0;
+    return std::atomic_compare_exchange_weak_explicit( aDest, &expected, int(1), std::memory_order_acq_rel, std::memory_order_acquire );
+}
+
+void btSpinMutex::lock()
+{
+    // note: this lock does not sleep the thread.
+    while (! tryLock())
+    {
+        // spin
+    }
+}
+
+void btSpinMutex::unlock()
+{
+    std::atomic<int>* aDest = reinterpret_cast<std::atomic<int>*>(&mLock);
+    std::atomic_store_explicit( aDest, int(0), std::memory_order_release );
+}
+
+
+#elif USE_MSVC_INTRINSICS
+
+#define WIN32_LEAN_AND_MEAN
+
+#include <windows.h>
+#include <intrin.h>
+
+#define THREAD_LOCAL_STATIC __declspec( thread ) static
+
+
+bool btSpinMutex::tryLock()
+{
+    volatile long* aDest = reinterpret_cast<long*>(&mLock);
+    return ( 0 == _InterlockedCompareExchange( aDest, 1, 0) );
+}
+
+void btSpinMutex::lock()
+{
+    // note: this lock does not sleep the thread
+    while (! tryLock())
+    {
+        // spin
+    }
+}
+
+void btSpinMutex::unlock()
+{
+    volatile long* aDest = reinterpret_cast<long*>( &mLock );
+    _InterlockedExchange( aDest, 0 );
+}
+
+#elif USE_GCC_BUILTIN_ATOMICS
+
+#define THREAD_LOCAL_STATIC static __thread
+
+
+bool btSpinMutex::tryLock()
+{
+    int expected = 0;
+    bool weak = false;
+    const int memOrderSuccess = __ATOMIC_ACQ_REL;
+    const int memOrderFail = __ATOMIC_ACQUIRE;
+    return __atomic_compare_exchange_n(&mLock, &expected, int(1), weak, memOrderSuccess, memOrderFail);
+}
+
+void btSpinMutex::lock()
+{
+    // note: this lock does not sleep the thread
+    while (! tryLock())
+    {
+        // spin
+    }
+}
+
+void btSpinMutex::unlock()
+{
+    __atomic_store_n(&mLock, int(0), __ATOMIC_RELEASE);
+}
+
+#elif USE_GCC_BUILTIN_ATOMICS_OLD
+
+
+#define THREAD_LOCAL_STATIC static __thread
+
+bool btSpinMutex::tryLock()
+{
+    return __sync_bool_compare_and_swap(&mLock, int(0), int(1));
+}
+
+void btSpinMutex::lock()
+{
+    // note: this lock does not sleep the thread
+    while (! tryLock())
+    {
+        // spin
+    }
+}
+
+void btSpinMutex::unlock()
+{
+    // write 0
+    __sync_fetch_and_and(&mLock, int(0));
+}
+
+#else //#elif USE_MSVC_INTRINSICS
+
+#error "no threading primitives defined -- unknown platform"
+
+#endif  //#else //#elif USE_MSVC_INTRINSICS
+
+
+struct ThreadsafeCounter
+{
+    unsigned int mCounter;
+    btSpinMutex mMutex;
+
+    ThreadsafeCounter() {mCounter=0;}
+
+    unsigned int getNext()
+    {
+        // no need to optimize this with atomics, it is only called ONCE per thread!
+        mMutex.lock();
+        unsigned int val = mCounter++;
+        mMutex.unlock();
+        return val;
+    }
+};
+
+static ThreadsafeCounter gThreadCounter;
+
+
+// return a unique index per thread, starting with 0 and counting up
+unsigned int btGetCurrentThreadIndex()
+{
+    const unsigned int kNullIndex = ~0U;
+    THREAD_LOCAL_STATIC unsigned int sThreadIndex = kNullIndex;
+    if ( sThreadIndex == kNullIndex )
+    {
+        sThreadIndex = gThreadCounter.getNext();
+    }
+    return sThreadIndex;
+}
+
+bool btIsMainThread()
+{
+    return btGetCurrentThreadIndex() == 0;
+}
+
+#else // #if BT_THREADSAFE
+
+// These should not be called ever
+void btSpinMutex::lock()
+{
+    btAssert(!"unimplemented btSpinMutex::lock() called");
+}
+
+void btSpinMutex::unlock()
+{
+    btAssert(!"unimplemented btSpinMutex::unlock() called");
+}
+
+bool btSpinMutex::tryLock()
+{
+    btAssert(!"unimplemented btSpinMutex::tryLock() called");
+    return true;
+}
+
+#endif // #else // #if BT_THREADSAFE
+

src/LinearMath/btThreads.h

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2003-2014 Erwin Coumans  http://bullet.googlecode.com
+
+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.
+*/
+
+
+
+#ifndef BT_THREADS_H
+#define BT_THREADS_H
+
+#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
+
+///
+/// btSpinMutex -- lightweight spin-mutex implemented with atomic ops, never puts
+///               a thread to sleep because it is designed to be used with a task scheduler
+///               which has one thread per core and the threads don't sleep until they
+///               run out of tasks. Not good for general purpose use.
+///
+class btSpinMutex
+{
+    int mLock;
+
+public:
+    btSpinMutex()
+    {
+        mLock = 0;
+    }
+    void lock();
+    void unlock();
+    bool tryLock();
+};
+
+#if BT_THREADSAFE
+
+// for internal Bullet use only
+SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* mutex )
+{
+    mutex->lock();
+}
+
+SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* mutex )
+{
+    mutex->unlock();
+}
+
+SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* mutex )
+{
+    return mutex->tryLock();
+}
+
+// for internal use only
+bool btIsMainThread();
+unsigned int btGetCurrentThreadIndex();
+const unsigned int BT_MAX_THREAD_COUNT = 64;
+
+#else
+
+// for internal Bullet use only
+// if BT_THREADSAFE is undefined or 0, should optimize away to nothing
+SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* ) {}
+SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* ) {}
+SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* ) {return true;}
+
+#endif
+
+
+#endif //BT_THREADS_H