Update Files

lib/haxebullet/bullet/BulletDynamics/Dynamics/btActionInterface.h

@@ -0,0 +1,46 @@
+/*
+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_ACTION_INTERFACE_H
+#define _BT_ACTION_INTERFACE_H
+
+class btIDebugDraw;
+class btCollisionWorld;
+
+#include "LinearMath/btScalar.h"
+#include "btRigidBody.h"
+
+///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
+class btActionInterface
+{
+protected:
+
+	static btRigidBody& getFixedBody();
+	
+	
+public:
+
+	virtual ~btActionInterface()
+	{
+	}
+
+	virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0;
+
+	virtual void debugDraw(btIDebugDraw* debugDrawer) = 0;
+
+};
+
+#endif //_BT_ACTION_INTERFACE_H
+

lib/haxebullet/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -0,0 +1,239 @@
+/*
+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_H
+#define BT_DISCRETE_DYNAMICS_WORLD_H
+
+#include "btDynamicsWorld.h"
+
+class btDispatcher;
+class btOverlappingPairCache;
+class btConstraintSolver;
+class btSimulationIslandManager;
+class btTypedConstraint;
+class btActionInterface;
+class btPersistentManifold;
+class btIDebugDraw;
+struct InplaceSolverIslandCallback;
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btThreads.h"
+
+
+///btDiscreteDynamicsWorld provides discrete rigid body simulation
+///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
+ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld
+{
+protected:
+	
+    btAlignedObjectArray<btTypedConstraint*>	m_sortedConstraints;
+	InplaceSolverIslandCallback* 	m_solverIslandCallback;
+
+	btConstraintSolver*	m_constraintSolver;
+
+	btSimulationIslandManager*	m_islandManager;
+
+	btAlignedObjectArray<btTypedConstraint*> m_constraints;
+
+	btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
+
+	btVector3	m_gravity;
+
+	//for variable timesteps
+	btScalar	m_localTime;
+	btScalar	m_fixedTimeStep;
+	//for variable timesteps
+
+	bool	m_ownsIslandManager;
+	bool	m_ownsConstraintSolver;
+	bool	m_synchronizeAllMotionStates;
+	bool	m_applySpeculativeContactRestitution;
+
+	btAlignedObjectArray<btActionInterface*>	m_actions;
+	
+	int	m_profileTimings;
+
+	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();
+
+	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+	
+	virtual void	updateActivationState(btScalar timeStep);
+
+	void	updateActions(btScalar timeStep);
+
+	void	startProfiling(btScalar timeStep);
+
+	virtual void	internalSingleStepSimulation( 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);
+
+	void	serializeRigidBodies(btSerializer* serializer);
+
+	void	serializeDynamicsWorldInfo(btSerializer* serializer);
+
+public:
+
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
+	btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btDiscreteDynamicsWorld();
+
+	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
+	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+
+
+	virtual void	synchronizeMotionStates();
+
+	///this can be useful to synchronize a single rigid body -> graphics object
+	void	synchronizeSingleMotionState(btRigidBody* body);
+
+	virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
+
+	virtual void	removeConstraint(btTypedConstraint* constraint);
+
+	virtual void	addAction(btActionInterface*);
+
+	virtual void	removeAction(btActionInterface*);
+	
+	btSimulationIslandManager*	getSimulationIslandManager()
+	{
+		return m_islandManager;
+	}
+
+	const btSimulationIslandManager*	getSimulationIslandManager() const 
+	{
+		return m_islandManager;
+	}
+
+	btCollisionWorld*	getCollisionWorld()
+	{
+		return this;
+	}
+
+	virtual void	setGravity(const btVector3& gravity);
+
+	virtual btVector3 getGravity () const;
+
+	virtual void	addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::StaticFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+	virtual void	addRigidBody(btRigidBody* body);
+
+	virtual void	addRigidBody(btRigidBody* body, int group, int mask);
+
+	virtual void	removeRigidBody(btRigidBody* body);
+
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+
+
+	virtual void	debugDrawConstraint(btTypedConstraint* constraint);
+
+	virtual void	debugDrawWorld();
+
+	virtual void	setConstraintSolver(btConstraintSolver* solver);
+
+	virtual btConstraintSolver* getConstraintSolver();
+	
+	virtual	int		getNumConstraints() const;
+
+	virtual btTypedConstraint* getConstraint(int index)	;
+
+	virtual const btTypedConstraint* getConstraint(int index) const;
+
+	
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return BT_DISCRETE_DYNAMICS_WORLD;
+	}
+	
+	///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
+	virtual void	clearForces();
+
+	///apply gravity, call this once per timestep
+	virtual void	applyGravity();
+
+	virtual void	setNumTasks(int numTasks)
+	{
+        (void) numTasks;
+	}
+
+	///obsolete, use updateActions instead
+	virtual void updateVehicles(btScalar timeStep)
+	{
+		updateActions(timeStep);
+	}
+
+	///obsolete, use addAction instead
+	virtual void	addVehicle(btActionInterface* vehicle);
+	///obsolete, use removeAction instead
+	virtual void	removeVehicle(btActionInterface* vehicle);
+	///obsolete, use addAction instead
+	virtual void	addCharacter(btActionInterface* character);
+	///obsolete, use removeAction instead
+	virtual void	removeCharacter(btActionInterface* character);
+
+	void	setSynchronizeAllMotionStates(bool synchronizeAll)
+	{
+		m_synchronizeAllMotionStates = synchronizeAll;
+	}
+	bool getSynchronizeAllMotionStates() const
+	{
+		return m_synchronizeAllMotionStates;
+	}
+
+	void setApplySpeculativeContactRestitution(bool enable)
+	{
+		m_applySpeculativeContactRestitution = enable;
+	}
+	
+	bool getApplySpeculativeContactRestitution() const
+	{
+		return m_applySpeculativeContactRestitution;
+	}
+
+	///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
+	virtual	void	serialize(btSerializer* serializer);
+
+	///Interpolate motion state between previous and current transform, instead of current and next transform.
+	///This can relieve discontinuities in the rendering, due to penetrations
+	void setLatencyMotionStateInterpolation(bool latencyInterpolation )
+	{
+		m_latencyMotionStateInterpolation = latencyInterpolation;
+	}
+	bool getLatencyMotionStateInterpolation() const
+	{
+		return m_latencyMotionStateInterpolation;
+	}
+};
+
+#endif //BT_DISCRETE_DYNAMICS_WORLD_H

lib/haxebullet/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp

@@ -0,0 +1,327 @@
+/*
+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 "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
+                              );
+    }
+
+};
+
+
+///
+/// btConstraintSolverPoolMt
+///
+
+btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver()
+{
+    int i = 0;
+#if BT_THREADSAFE
+    i = btGetCurrentThreadIndex() % m_solvers.size();
+#endif // #if BT_THREADSAFE
+    while ( true )
+    {
+        ThreadSolver& solver = m_solvers[ i ];
+        if ( solver.mutex.tryLock() )
+        {
+            return &solver;
+        }
+        // failed, try the next one
+        i = ( i + 1 ) % m_solvers.size();
+    }
+    return NULL;
+}
+
+void btConstraintSolverPoolMt::init( btConstraintSolver** solvers, int numSolvers )
+{
+    m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
+    m_solvers.resize( numSolvers );
+    for ( int i = 0; i < numSolvers; ++i )
+    {
+        m_solvers[ i ].solver = solvers[ i ];
+    }
+    if ( numSolvers > 0 )
+    {
+        m_solverType = solvers[ 0 ]->getSolverType();
+    }
+}
+
+// create the solvers for me
+btConstraintSolverPoolMt::btConstraintSolverPoolMt( int numSolvers )
+{
+    btAlignedObjectArray<btConstraintSolver*> solvers;
+    solvers.reserve( numSolvers );
+    for ( int i = 0; i < numSolvers; ++i )
+    {
+        btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
+        solvers.push_back( solver );
+    }
+    init( &solvers[ 0 ], numSolvers );
+}
+
+// pass in fully constructed solvers (destructor will delete them)
+btConstraintSolverPoolMt::btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers )
+{
+    init( solvers, numSolvers );
+}
+
+btConstraintSolverPoolMt::~btConstraintSolverPoolMt()
+{
+    // delete all solvers
+    for ( int i = 0; i < m_solvers.size(); ++i )
+    {
+        ThreadSolver& solver = m_solvers[ i ];
+        delete solver.solver;
+        solver.solver = NULL;
+    }
+}
+
+///solve a group of constraints
+btScalar btConstraintSolverPoolMt::solveGroup( btCollisionObject** bodies,
+    int numBodies,
+    btPersistentManifold** manifolds,
+    int numManifolds,
+    btTypedConstraint** constraints,
+    int numConstraints,
+    const btContactSolverInfo& info,
+    btIDebugDraw* debugDrawer,
+    btDispatcher* dispatcher
+)
+{
+    ThreadSolver* ts = getAndLockThreadSolver();
+    ts->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher );
+    ts->mutex.unlock();
+    return 0.0f;
+}
+
+void btConstraintSolverPoolMt::reset()
+{
+    for ( int i = 0; i < m_solvers.size(); ++i )
+    {
+        ThreadSolver& solver = m_solvers[ i ];
+        solver.mutex.lock();
+        solver.solver->reset();
+        solver.mutex.unlock();
+    }
+}
+
+
+///
+/// btDiscreteDynamicsWorldMt
+///
+
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolverPoolMt* 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();
+        im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
+        m_islandManager = im;
+	}
+}
+
+
+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);
+}
+
+
+struct UpdaterUnconstrainedMotion : public btIParallelForBody
+{
+    btScalar timeStep;
+    btRigidBody** rigidBodies;
+
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        for ( int i = iBegin; i < iEnd; ++i )
+        {
+            btRigidBody* body = rigidBodies[ i ];
+            if ( !body->isStaticOrKinematicObject() )
+            {
+                //don't integrate/update velocities here, it happens in the constraint solver
+                body->applyDamping( timeStep );
+                body->predictIntegratedTransform( timeStep, body->getInterpolationWorldTransform() );
+            }
+        }
+    }
+};
+
+
+void btDiscreteDynamicsWorldMt::predictUnconstraintMotion( btScalar timeStep )
+{
+    BT_PROFILE( "predictUnconstraintMotion" );
+    if ( m_nonStaticRigidBodies.size() > 0 )
+    {
+        UpdaterUnconstrainedMotion update;
+        update.timeStep = timeStep;
+        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
+        int grainSize = 50;  // num of iterations per task for task scheduler
+        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
+    }
+}
+
+
+void btDiscreteDynamicsWorldMt::createPredictiveContacts( btScalar timeStep )
+{
+    BT_PROFILE( "createPredictiveContacts" );
+    releasePredictiveContacts();
+    if ( m_nonStaticRigidBodies.size() > 0 )
+    {
+        UpdaterCreatePredictiveContacts update;
+        update.world = this;
+        update.timeStep = timeStep;
+        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
+        int grainSize = 50;  // num of iterations per task for task scheduler
+        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
+    }
+}
+
+
+void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep )
+{
+    BT_PROFILE( "integrateTransforms" );
+    if ( m_nonStaticRigidBodies.size() > 0 )
+    {
+        UpdaterIntegrateTransforms update;
+        update.world = this;
+        update.timeStep = timeStep;
+        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
+        int grainSize = 50;  // num of iterations per task for task scheduler
+        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
+    }
+}
+

lib/haxebullet/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h

@@ -0,0 +1,134 @@
+/*
+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"
+#include "btSimulationIslandManagerMt.h"
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
+
+struct InplaceSolverIslandCallbackMt;
+
+///
+/// btConstraintSolverPoolMt - 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 btConstraintSolverPoolMt : public btConstraintSolver
+{
+public:
+    // create the solvers for me
+    explicit btConstraintSolverPoolMt( int numSolvers );
+
+    // pass in fully constructed solvers (destructor will delete them)
+    btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers );
+
+    virtual ~btConstraintSolverPoolMt();
+
+    ///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
+    ) BT_OVERRIDE;
+
+    virtual	void reset() BT_OVERRIDE;
+    virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; }
+
+private:
+    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();
+    void init( btConstraintSolver** solvers, int numSolvers );
+};
+
+
+
+///
+/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
+///                              solving simulation islands on multiple threads.
+///
+///  Should function exactly like btDiscreteDynamicsWorld.
+///  Also 3 methods that iterate over all of the rigidbodies can run in parallel:
+///     - predictUnconstraintMotion
+///     - integrateTransforms
+///     - createPredictiveContacts
+///
+ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
+{
+protected:
+    InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
+
+    virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE;
+
+    virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE;
+
+    struct UpdaterCreatePredictiveContacts : public btIParallelForBody
+    {
+        btScalar timeStep;
+        btRigidBody** rigidBodies;
+        btDiscreteDynamicsWorldMt* world;
+
+        void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+        {
+            world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
+        }
+    };
+    virtual void createPredictiveContacts( btScalar timeStep ) BT_OVERRIDE;
+
+    struct UpdaterIntegrateTransforms : public btIParallelForBody
+    {
+        btScalar timeStep;
+        btRigidBody** rigidBodies;
+        btDiscreteDynamicsWorldMt* world;
+
+        void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+        {
+            world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
+        }
+    };
+    virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
+        btBroadphaseInterface* pairCache,
+        btConstraintSolverPoolMt* constraintSolver,   // Note this should be a solver-pool for multi-threading
+        btCollisionConfiguration* collisionConfiguration
+    );
+	virtual ~btDiscreteDynamicsWorldMt();
+};
+
+#endif //BT_DISCRETE_DYNAMICS_WORLD_H

lib/haxebullet/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

@@ -0,0 +1,173 @@
+/*
+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_DYNAMICS_WORLD_H
+#define BT_DYNAMICS_WORLD_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+class btTypedConstraint;
+class btActionInterface;
+class btConstraintSolver;
+class btDynamicsWorld;
+
+
+/// Type for the callback for each tick
+typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
+
+enum btDynamicsWorldType
+{
+	BT_SIMPLE_DYNAMICS_WORLD=1,
+	BT_DISCRETE_DYNAMICS_WORLD=2,
+	BT_CONTINUOUS_DYNAMICS_WORLD=3,
+	BT_SOFT_RIGID_DYNAMICS_WORLD=4,
+	BT_GPU_DYNAMICS_WORLD=5,
+    BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6
+};
+
+///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
+class btDynamicsWorld : public btCollisionWorld
+{
+
+protected:
+		btInternalTickCallback m_internalTickCallback;
+		btInternalTickCallback m_internalPreTickCallback;
+		void*	m_worldUserInfo;
+
+		btContactSolverInfo	m_solverInfo;
+
+public:
+		
+
+		btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
+		:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
+		{
+		}
+
+		virtual ~btDynamicsWorld()
+		{
+		}
+		
+		///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
+		///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
+		///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
+		///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
+		virtual int		stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
+			
+		virtual void	debugDrawWorld() = 0;
+				
+		virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) 
+		{ 
+			(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
+		}
+
+		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
+
+		virtual void	addAction(btActionInterface* action) = 0;
+
+		virtual void	removeAction(btActionInterface* action) = 0;
+
+		//once a rigidbody is added to the dynamics world, it will get this gravity assigned
+		//existing rigidbodies in the world get gravity assigned too, during this method
+		virtual void	setGravity(const btVector3& gravity) = 0;
+		virtual btVector3 getGravity () const = 0;
+
+		virtual void	synchronizeMotionStates() = 0;
+
+		virtual void	addRigidBody(btRigidBody* body) = 0;
+
+		virtual void	addRigidBody(btRigidBody* body, int group, int mask) = 0;
+
+		virtual void	removeRigidBody(btRigidBody* body) = 0;
+
+		virtual void	setConstraintSolver(btConstraintSolver* solver) = 0;
+
+		virtual btConstraintSolver* getConstraintSolver() = 0;
+		
+		virtual	int		getNumConstraints() const {	return 0;		}
+		
+		virtual btTypedConstraint* getConstraint(int index)		{	(void)index;		return 0;		}
+		
+		virtual const btTypedConstraint* getConstraint(int index) const	{	(void)index;	return 0;	}
+
+		virtual btDynamicsWorldType	getWorldType() const=0;
+
+		virtual void	clearForces() = 0;
+
+		/// Set the callback for when an internal tick (simulation substep) happens, optional user info
+		void setInternalTickCallback(btInternalTickCallback cb,	void* worldUserInfo=0,bool isPreTick=false) 
+		{ 
+			if (isPreTick)
+			{
+				m_internalPreTickCallback = cb;
+			} else
+			{
+				m_internalTickCallback = cb; 
+			}
+			m_worldUserInfo = worldUserInfo;
+		}
+
+		void	setWorldUserInfo(void* worldUserInfo)
+		{
+			m_worldUserInfo = worldUserInfo;
+		}
+
+		void*	getWorldUserInfo() const
+		{
+			return m_worldUserInfo;
+		}
+
+		btContactSolverInfo& getSolverInfo()
+		{
+			return m_solverInfo;
+		}
+
+		const btContactSolverInfo& getSolverInfo() const
+		{
+			return m_solverInfo;
+		}
+
+
+		///obsolete, use addAction instead.
+		virtual void	addVehicle(btActionInterface* vehicle) {(void)vehicle;}
+		///obsolete, use removeAction instead
+		virtual void	removeVehicle(btActionInterface* vehicle) {(void)vehicle;}
+		///obsolete, use addAction instead.
+		virtual void	addCharacter(btActionInterface* character) {(void)character;}
+		///obsolete, use removeAction instead
+		virtual void	removeCharacter(btActionInterface* character) {(void)character;}
+
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btDynamicsWorldDoubleData
+{
+	btContactSolverInfoDoubleData	m_solverInfo;
+	btVector3DoubleData	m_gravity;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btDynamicsWorldFloatData
+{
+	btContactSolverInfoFloatData	m_solverInfo;
+	btVector3FloatData	m_gravity;
+};
+
+
+#endif //BT_DYNAMICS_WORLD_H
+
+

lib/haxebullet/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -0,0 +1,527 @@
+/*
+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 "btRigidBody.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btMotionState.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "LinearMath/btSerializer.h"
+
+//'temporarily' global variables
+btScalar	gDeactivationTime = btScalar(2.);
+bool	gDisableDeactivation = false;
+static int uniqueId = 0;
+
+
+btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+{
+	setupRigidBody(constructionInfo);
+}
+
+btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
+{
+	btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
+	setupRigidBody(cinfo);
+}
+
+void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+{
+
+	m_internalType=CO_RIGID_BODY;
+
+	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	m_angularFactor.setValue(1,1,1);
+	m_linearFactor.setValue(1,1,1);
+	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
+    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
+
+	m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
+	m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
+	m_optionalMotionState = constructionInfo.m_motionState;
+	m_contactSolverType = 0;
+	m_frictionSolverType = 0;
+	m_additionalDamping = constructionInfo.m_additionalDamping;
+	m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
+	m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
+	m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
+	m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
+
+	if (m_optionalMotionState)
+	{
+		m_optionalMotionState->getWorldTransform(m_worldTransform);
+	} else
+	{
+		m_worldTransform = constructionInfo.m_startWorldTransform;
+	}
+
+	m_interpolationWorldTransform = m_worldTransform;
+	m_interpolationLinearVelocity.setValue(0,0,0);
+	m_interpolationAngularVelocity.setValue(0,0,0);
+	
+	//moved to btCollisionObject
+	m_friction = constructionInfo.m_friction;
+	m_rollingFriction = constructionInfo.m_rollingFriction;
+    m_spinningFriction = constructionInfo.m_spinningFriction;
+    
+	m_restitution = constructionInfo.m_restitution;
+
+	setCollisionShape( constructionInfo.m_collisionShape );
+	m_debugBodyId = uniqueId++;
+	
+	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
+	updateInertiaTensor();
+
+	m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
+
+
+	m_deltaLinearVelocity.setZero();
+	m_deltaAngularVelocity.setZero();
+	m_invMass = m_inverseMass*m_linearFactor;
+	m_pushVelocity.setZero();
+	m_turnVelocity.setZero();
+
+	
+
+}
+
+
+void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
+{
+	btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
+}
+
+void			btRigidBody::saveKinematicState(btScalar timeStep)
+{
+	//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
+	if (timeStep != btScalar(0.))
+	{
+		//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
+		if (getMotionState())
+			getMotionState()->getWorldTransform(m_worldTransform);
+		btVector3 linVel,angVel;
+		
+		btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
+		m_interpolationLinearVelocity = m_linearVelocity;
+		m_interpolationAngularVelocity = m_angularVelocity;
+		m_interpolationWorldTransform = m_worldTransform;
+		//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
+	}
+}
+	
+void	btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
+{
+	getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
+}
+
+
+
+
+void btRigidBody::setGravity(const btVector3& acceleration) 
+{
+	if (m_inverseMass != btScalar(0.0))
+	{
+		m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
+	}
+	m_gravity_acceleration = acceleration;
+}
+
+
+
+
+
+
+void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
+{
+	m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+}
+
+
+
+
+///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
+void			btRigidBody::applyDamping(btScalar timeStep)
+{
+	//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
+	//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
+
+//#define USE_OLD_DAMPING_METHOD 1
+#ifdef USE_OLD_DAMPING_METHOD
+	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#else
+	m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
+	m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
+#endif
+
+	if (m_additionalDamping)
+	{
+		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+		if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
+			(m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
+		{
+			m_angularVelocity *= m_additionalDampingFactor;
+			m_linearVelocity *= m_additionalDampingFactor;
+		}
+	
+
+		btScalar speed = m_linearVelocity.length();
+		if (speed < m_linearDamping)
+		{
+			btScalar dampVel = btScalar(0.005);
+			if (speed > dampVel)
+			{
+				btVector3 dir = m_linearVelocity.normalized();
+				m_linearVelocity -=  dir * dampVel;
+			} else
+			{
+				m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			}
+		}
+
+		btScalar angSpeed = m_angularVelocity.length();
+		if (angSpeed < m_angularDamping)
+		{
+			btScalar angDampVel = btScalar(0.005);
+			if (angSpeed > angDampVel)
+			{
+				btVector3 dir = m_angularVelocity.normalized();
+				m_angularVelocity -=  dir * angDampVel;
+			} else
+			{
+				m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			}
+		}
+	}
+}
+
+
+void btRigidBody::applyGravity()
+{
+	if (isStaticOrKinematicObject())
+		return;
+	
+	applyCentralForce(m_gravity);	
+
+}
+
+void btRigidBody::proceedToTransform(const btTransform& newTrans)
+{
+	setCenterOfMassTransform( newTrans );
+}
+	
+
+void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
+{
+	if (mass == btScalar(0.))
+	{
+		m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
+		m_inverseMass = btScalar(0.);
+	} else
+	{
+		m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
+		m_inverseMass = btScalar(1.0) / mass;
+	}
+
+	//Fg = m * a
+	m_gravity = mass * m_gravity_acceleration;
+	
+	m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
+				   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
+				   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
+
+	m_invMass = m_linearFactor*m_inverseMass;
+}
+
+	
+void btRigidBody::updateInertiaTensor() 
+{
+	m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
+}
+
+
+
+btVector3 btRigidBody::getLocalInertia() const
+{
+
+	btVector3 inertiaLocal;
+	const btVector3 inertia = m_invInertiaLocal;
+	inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
+		inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
+		inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
+	return inertiaLocal;
+}
+
+inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
+	const btMatrix3x3 &I)
+{
+	const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
+	return w2;
+}
+
+inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
+	const btMatrix3x3 &I)
+{
+
+	btMatrix3x3 w1x, Iw1x;
+	const btVector3 Iwi = (I*w1);
+	w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
+	Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
+
+	const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt;
+	return dfw1;
+}
+
+btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
+{
+	btVector3 inertiaLocal = getLocalInertia();
+	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
+	btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
+	btVector3 gf = getAngularVelocity().cross(tmp);
+	btScalar l2 = gf.length2();
+	if (l2>maxGyroscopicForce*maxGyroscopicForce)
+	{
+		gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce;
+	}
+	return gf;
+}
+
+
+btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
+{	
+	btVector3 idl = getLocalInertia();
+	btVector3 omega1 = getAngularVelocity();
+	btQuaternion q = getWorldTransform().getRotation();
+	
+	// Convert to body coordinates
+	btVector3 omegab = quatRotate(q.inverse(), omega1);
+	btMatrix3x3 Ib;
+	Ib.setValue(idl.x(),0,0,
+				0,idl.y(),0,
+				0,0,idl.z());
+	
+	btVector3 ibo = Ib*omegab;
+
+	// Residual vector
+	btVector3 f = step * omegab.cross(ibo);
+	
+	btMatrix3x3 skew0;
+	omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
+	btVector3 om = Ib*omegab;
+	btMatrix3x3 skew1;
+	om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
+	
+	// Jacobian
+	btMatrix3x3 J = Ib +  (skew0*Ib - skew1)*step;
+	
+//	btMatrix3x3 Jinv = J.inverse();
+//	btVector3 omega_div = Jinv*f;
+	btVector3 omega_div = J.solve33(f);
+	
+	// Single Newton-Raphson update
+	omegab = omegab - omega_div;//Solve33(J, f);
+	// Back to world coordinates
+	btVector3 omega2 = quatRotate(q,omegab);
+	btVector3 gf = omega2-omega1;
+	return gf;
+}
+
+
+
+btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
+{
+	// use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
+	// calculate using implicit euler step so it's stable.
+
+	const btVector3 inertiaLocal = getLocalInertia();
+	const btVector3 w0 = getAngularVelocity();
+
+	btMatrix3x3 I;
+
+	I = m_worldTransform.getBasis().scaled(inertiaLocal) *
+		m_worldTransform.getBasis().transpose();
+
+	// use newtons method to find implicit solution for new angular velocity (w')
+	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 
+	// df/dw' = I + 1xIw'*step + w'xI*step
+
+	btVector3 w1 = w0;
+
+	// one step of newton's method
+	{
+		const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
+		const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
+
+		btVector3 dw;
+		dw = dfw.solve33(fw);
+		//const btMatrix3x3 dfw_inv = dfw.inverse();
+		//dw = dfw_inv*fw;
+
+		w1 -= dw;
+	}
+
+	btVector3 gf = (w1 - w0);
+	return gf;
+}
+
+
+void btRigidBody::integrateVelocities(btScalar step) 
+{
+	if (isStaticOrKinematicObject())
+		return;
+
+	m_linearVelocity += m_totalForce * (m_inverseMass * step);
+	m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
+
+#define MAX_ANGVEL SIMD_HALF_PI
+	/// clamp angular velocity. collision calculations will fail on higher angular velocities	
+	btScalar angvel = m_angularVelocity.length();
+	if (angvel*step > MAX_ANGVEL)
+	{
+		m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
+	}
+
+}
+
+btQuaternion btRigidBody::getOrientation() const
+{
+		btQuaternion orn;
+		m_worldTransform.getBasis().getRotation(orn);
+		return orn;
+}
+	
+	
+void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
+{
+
+	if (isKinematicObject())
+	{
+		m_interpolationWorldTransform = m_worldTransform;
+	} else
+	{
+		m_interpolationWorldTransform = xform;
+	}
+	m_interpolationLinearVelocity = getLinearVelocity();
+	m_interpolationAngularVelocity = getAngularVelocity();
+	m_worldTransform = xform;
+	updateInertiaTensor();
+}
+
+
+
+
+
+void btRigidBody::addConstraintRef(btTypedConstraint* c)
+{
+	///disable collision with the 'other' body
+
+	int index = m_constraintRefs.findLinearSearch(c);
+	//don't add constraints that are already referenced
+	//btAssert(index == m_constraintRefs.size());
+	if (index == m_constraintRefs.size())
+	{
+		m_constraintRefs.push_back(c);
+		btCollisionObject* colObjA = &c->getRigidBodyA();
+		btCollisionObject* colObjB = &c->getRigidBodyB();
+		if (colObjA == this)
+		{
+			colObjA->setIgnoreCollisionCheck(colObjB, true);
+		}
+		else
+		{
+			colObjB->setIgnoreCollisionCheck(colObjA, true);
+		}
+	} 
+}
+
+void btRigidBody::removeConstraintRef(btTypedConstraint* c)
+{
+	int index = m_constraintRefs.findLinearSearch(c);
+	//don't remove constraints that are not referenced
+	if(index < m_constraintRefs.size())
+    {
+        m_constraintRefs.remove(c);
+        btCollisionObject* colObjA = &c->getRigidBodyA();
+        btCollisionObject* colObjB = &c->getRigidBodyB();
+        if (colObjA == this)
+        {
+            colObjA->setIgnoreCollisionCheck(colObjB, false);
+        }
+        else
+        {
+            colObjB->setIgnoreCollisionCheck(colObjA, false);
+        }
+    }
+}
+
+int	btRigidBody::calculateSerializeBufferSize()	const
+{
+	int sz = sizeof(btRigidBodyData);
+	return sz;
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+{
+	btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
+
+	btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
+
+	m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
+	m_linearVelocity.serialize(rbd->m_linearVelocity);
+	m_angularVelocity.serialize(rbd->m_angularVelocity);
+	rbd->m_inverseMass = m_inverseMass;
+	m_angularFactor.serialize(rbd->m_angularFactor);
+	m_linearFactor.serialize(rbd->m_linearFactor);
+	m_gravity.serialize(rbd->m_gravity);
+	m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
+	m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
+	m_totalForce.serialize(rbd->m_totalForce);
+	m_totalTorque.serialize(rbd->m_totalTorque);
+	rbd->m_linearDamping = m_linearDamping;
+	rbd->m_angularDamping = m_angularDamping;
+	rbd->m_additionalDamping = m_additionalDamping;
+	rbd->m_additionalDampingFactor = m_additionalDampingFactor;
+	rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
+	rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
+	rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
+	rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
+	rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
+
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	memset(rbd->m_padding, 0, sizeof(rbd->m_padding));
+#endif
+
+	return btRigidBodyDataName;
+}
+
+
+
+void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
+{
+	btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
+	const char* structType = serialize(chunk->m_oldPtr, serializer);
+	serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
+}
+
+

lib/haxebullet/bullet/BulletDynamics/Dynamics/btRigidBody.h

@@ -0,0 +1,619 @@
+/*
+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_RIGIDBODY_H
+#define BT_RIGIDBODY_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+class btCollisionShape;
+class btMotionState;
+class btTypedConstraint;
+
+
+extern btScalar gDeactivationTime;
+extern bool gDisableDeactivation;
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btRigidBodyData	btRigidBodyDoubleData
+#define btRigidBodyDataName	"btRigidBodyDoubleData"
+#else
+#define btRigidBodyData	btRigidBodyFloatData
+#define btRigidBodyDataName	"btRigidBodyFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+
+enum	btRigidBodyFlags
+{
+	BT_DISABLE_WORLD_GRAVITY = 1,
+	///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
+	///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
+	///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
+	BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8,
+	BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
+};
+
+
+///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
+///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
+///There are 3 types of rigid bodies: 
+///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
+///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
+///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
+///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
+///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
+class btRigidBody  : public btCollisionObject
+{
+
+	btMatrix3x3	m_invInertiaTensorWorld;
+	btVector3		m_linearVelocity;
+	btVector3		m_angularVelocity;
+	btScalar		m_inverseMass;
+	btVector3		m_linearFactor;
+
+	btVector3		m_gravity;	
+	btVector3		m_gravity_acceleration;
+	btVector3		m_invInertiaLocal;
+	btVector3		m_totalForce;
+	btVector3		m_totalTorque;
+	
+	btScalar		m_linearDamping;
+	btScalar		m_angularDamping;
+
+	bool			m_additionalDamping;
+	btScalar		m_additionalDampingFactor;
+	btScalar		m_additionalLinearDampingThresholdSqr;
+	btScalar		m_additionalAngularDampingThresholdSqr;
+	btScalar		m_additionalAngularDampingFactor;
+
+
+	btScalar		m_linearSleepingThreshold;
+	btScalar		m_angularSleepingThreshold;
+
+	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
+	btMotionState*	m_optionalMotionState;
+
+	//keep track of typed constraints referencing this rigid body, to disable collision between linked bodies
+	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
+
+	int				m_rigidbodyFlags;
+	
+	int				m_debugBodyId;
+	
+
+protected:
+
+	ATTRIBUTE_ALIGNED16(btVector3		m_deltaLinearVelocity);
+	btVector3		m_deltaAngularVelocity;
+	btVector3		m_angularFactor;
+	btVector3		m_invMass;
+	btVector3		m_pushVelocity;
+	btVector3		m_turnVelocity;
+
+
+public:
+
+
+	///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
+	///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
+	///You can use the motion state to synchronize the world transform between physics and graphics objects. 
+	///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
+	///m_startWorldTransform is only used when you don't provide a motion state.
+	struct	btRigidBodyConstructionInfo
+	{
+		btScalar			m_mass;
+
+		///When a motionState is provided, the rigid body will initialize its world transform from the motion state
+		///In this case, m_startWorldTransform is ignored.
+		btMotionState*		m_motionState;
+		btTransform	m_startWorldTransform;
+
+		btCollisionShape*	m_collisionShape;
+		btVector3			m_localInertia;
+		btScalar			m_linearDamping;
+		btScalar			m_angularDamping;
+
+		///best simulation results when friction is non-zero
+		btScalar			m_friction;
+		///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
+		///See Bullet/Demos/RollingFrictionDemo for usage
+		btScalar			m_rollingFriction;
+        btScalar			m_spinningFriction;//torsional friction around contact normal
+        
+		///best simulation results using zero restitution.
+		btScalar			m_restitution;
+
+		btScalar			m_linearSleepingThreshold;
+		btScalar			m_angularSleepingThreshold;
+
+		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+		bool				m_additionalDamping;
+		btScalar			m_additionalDampingFactor;
+		btScalar			m_additionalLinearDampingThresholdSqr;
+		btScalar			m_additionalAngularDampingThresholdSqr;
+		btScalar			m_additionalAngularDampingFactor;
+
+		btRigidBodyConstructionInfo(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
+		m_mass(mass),
+			m_motionState(motionState),
+			m_collisionShape(collisionShape),
+			m_localInertia(localInertia),
+			m_linearDamping(btScalar(0.)),
+			m_angularDamping(btScalar(0.)),
+			m_friction(btScalar(0.5)),
+			m_rollingFriction(btScalar(0)),
+            m_spinningFriction(btScalar(0)),
+			m_restitution(btScalar(0.)),
+			m_linearSleepingThreshold(btScalar(0.8)),
+			m_angularSleepingThreshold(btScalar(1.f)),
+			m_additionalDamping(false),
+			m_additionalDampingFactor(btScalar(0.005)),
+			m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
+			m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
+			m_additionalAngularDampingFactor(btScalar(0.01))
+		{
+			m_startWorldTransform.setIdentity();
+		}
+	};
+
+	///btRigidBody constructor using construction info
+	btRigidBody(	const btRigidBodyConstructionInfo& constructionInfo);
+
+	///btRigidBody constructor for backwards compatibility. 
+	///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
+	btRigidBody(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
+
+
+	virtual ~btRigidBody()
+        { 
+                //No constraints should point to this rigidbody
+		//Remove constraints from the dynamics world before you delete the related rigidbodies. 
+                btAssert(m_constraintRefs.size()==0); 
+        }
+
+protected:
+
+	///setupRigidBody is only used internally by the constructor
+	void	setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
+
+public:
+
+	void			proceedToTransform(const btTransform& newTrans); 
+	
+	///to keep collision detection and dynamics separate we don't store a rigidbody pointer
+	///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
+	static const btRigidBody*	upcast(const btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
+			return (const btRigidBody*)colObj;
+		return 0;
+	}
+	static btRigidBody*	upcast(btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
+			return (btRigidBody*)colObj;
+		return 0;
+	}
+	
+	/// continuous collision detection needs prediction
+	void			predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
+	
+	void			saveKinematicState(btScalar step);
+	
+	void			applyGravity();
+	
+	void			setGravity(const btVector3& acceleration);  
+
+	const btVector3&	getGravity() const
+	{
+		return m_gravity_acceleration;
+	}
+
+	void			setDamping(btScalar lin_damping, btScalar ang_damping);
+
+	btScalar getLinearDamping() const
+	{
+		return m_linearDamping;
+	}
+
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+
+	btScalar getLinearSleepingThreshold() const
+	{
+		return m_linearSleepingThreshold;
+	}
+
+	btScalar getAngularSleepingThreshold() const
+	{
+		return m_angularSleepingThreshold;
+	}
+
+	void			applyDamping(btScalar timeStep);
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const {
+		return m_collisionShape;
+	}
+
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape() {
+			return m_collisionShape;
+	}
+	
+	void			setMassProps(btScalar mass, const btVector3& inertia);
+	
+	const btVector3& getLinearFactor() const
+	{
+		return m_linearFactor;
+	}
+	void setLinearFactor(const btVector3& linearFactor)
+	{
+		m_linearFactor = linearFactor;
+		m_invMass = m_linearFactor*m_inverseMass;
+	}
+	btScalar		getInvMass() const { return m_inverseMass; }
+	const btMatrix3x3& getInvInertiaTensorWorld() const { 
+		return m_invInertiaTensorWorld; 
+	}
+		
+	void			integrateVelocities(btScalar step);
+
+	void			setCenterOfMassTransform(const btTransform& xform);
+
+	void			applyCentralForce(const btVector3& force)
+	{
+		m_totalForce += force*m_linearFactor;
+	}
+
+	const btVector3& getTotalForce() const
+	{
+		return m_totalForce;
+	};
+
+	const btVector3& getTotalTorque() const
+	{
+		return m_totalTorque;
+	};
+    
+	const btVector3& getInvInertiaDiagLocal() const
+	{
+		return m_invInertiaLocal;
+	};
+
+	void	setInvInertiaDiagLocal(const btVector3& diagInvInertia)
+	{
+		m_invInertiaLocal = diagInvInertia;
+	}
+
+	void	setSleepingThresholds(btScalar linear,btScalar angular)
+	{
+		m_linearSleepingThreshold = linear;
+		m_angularSleepingThreshold = angular;
+	}
+
+	void	applyTorque(const btVector3& torque)
+	{
+		m_totalTorque += torque*m_angularFactor;
+	}
+	
+	void	applyForce(const btVector3& force, const btVector3& rel_pos) 
+	{
+		applyCentralForce(force);
+		applyTorque(rel_pos.cross(force*m_linearFactor));
+	}
+	
+	void applyCentralImpulse(const btVector3& impulse)
+	{
+		m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
+	}
+	
+  	void applyTorqueImpulse(const btVector3& torque)
+	{
+			m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+	}
+	
+	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) 
+	{
+		if (m_inverseMass != btScalar(0.))
+		{
+			applyCentralImpulse(impulse);
+			if (m_angularFactor)
+			{
+				applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
+			}
+		}
+	}
+
+	void clearForces() 
+	{
+		m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+		m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	}
+	
+	void updateInertiaTensor();    
+	
+	const btVector3&     getCenterOfMassPosition() const { 
+		return m_worldTransform.getOrigin(); 
+	}
+	btQuaternion getOrientation() const;
+	
+	const btTransform&  getCenterOfMassTransform() const { 
+		return m_worldTransform; 
+	}
+	const btVector3&   getLinearVelocity() const { 
+		return m_linearVelocity; 
+	}
+	const btVector3&    getAngularVelocity() const { 
+		return m_angularVelocity; 
+	}
+	
+
+	inline void setLinearVelocity(const btVector3& lin_vel)
+	{ 
+		m_updateRevision++;
+		m_linearVelocity = lin_vel; 
+	}
+
+	inline void setAngularVelocity(const btVector3& ang_vel) 
+	{ 
+		m_updateRevision++;
+		m_angularVelocity = ang_vel; 
+	}
+
+	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
+	{
+		//we also calculate lin/ang velocity for kinematic objects
+		return m_linearVelocity + m_angularVelocity.cross(rel_pos);
+
+		//for kinematic objects, we could also use use:
+		//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
+	}
+
+	void translate(const btVector3& v) 
+	{
+		m_worldTransform.getOrigin() += v; 
+	}
+
+	
+	void	getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+
+
+	
+	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
+	{
+		btVector3 r0 = pos - getCenterOfMassPosition();
+
+		btVector3 c0 = (r0).cross(normal);
+
+		btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
+
+		return m_inverseMass + normal.dot(vec);
+
+	}
+
+	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
+	{
+		btVector3 vec = axis * getInvInertiaTensorWorld();
+		return axis.dot(vec);
+	}
+
+	SIMD_FORCE_INLINE void	updateDeactivation(btScalar timeStep)
+	{
+		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
+			return;
+
+		if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
+			(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
+		{
+			m_deactivationTime += timeStep;
+		} else
+		{
+			m_deactivationTime=btScalar(0.);
+			setActivationState(0);
+		}
+
+	}
+
+	SIMD_FORCE_INLINE bool	wantsSleeping()
+	{
+
+		if (getActivationState() == DISABLE_DEACTIVATION)
+			return false;
+
+		//disable deactivation
+		if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
+			return false;
+
+		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
+			return true;
+
+		if (m_deactivationTime> gDeactivationTime)
+		{
+			return true;
+		}
+		return false;
+	}
+
+
+	
+	const btBroadphaseProxy*	getBroadphaseProxy() const
+	{
+		return m_broadphaseHandle;
+	}
+	btBroadphaseProxy*	getBroadphaseProxy() 
+	{
+		return m_broadphaseHandle;
+	}
+	void	setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
+	{
+		m_broadphaseHandle = broadphaseProxy;
+	}
+
+	//btMotionState allows to automatic synchronize the world transform for active objects
+	btMotionState*	getMotionState()
+	{
+		return m_optionalMotionState;
+	}
+	const btMotionState*	getMotionState() const
+	{
+		return m_optionalMotionState;
+	}
+	void	setMotionState(btMotionState* motionState)
+	{
+		m_optionalMotionState = motionState;
+		if (m_optionalMotionState)
+			motionState->getWorldTransform(m_worldTransform);
+	}
+
+	//for experimental overriding of friction/contact solver func
+	int	m_contactSolverType;
+	int	m_frictionSolverType;
+
+	void	setAngularFactor(const btVector3& angFac)
+	{
+		m_updateRevision++;
+		m_angularFactor = angFac;
+	}
+
+	void	setAngularFactor(btScalar angFac)
+	{
+		m_updateRevision++;
+		m_angularFactor.setValue(angFac,angFac,angFac);
+	}
+	const btVector3&	getAngularFactor() const
+	{
+		return m_angularFactor;
+	}
+
+	//is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
+	bool	isInWorld() const
+	{
+		return (getBroadphaseProxy() != 0);
+	}
+
+	void addConstraintRef(btTypedConstraint* c);
+	void removeConstraintRef(btTypedConstraint* c);
+
+	btTypedConstraint* getConstraintRef(int index)
+	{
+		return m_constraintRefs[index];
+	}
+
+	int getNumConstraintRefs() const
+	{
+		return m_constraintRefs.size();
+	}
+
+	void	setFlags(int flags)
+	{
+		m_rigidbodyFlags = flags;
+	}
+
+	int getFlags() const
+	{
+		return m_rigidbodyFlags;
+	}
+
+
+	
+
+	///perform implicit force computation in world space
+	btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const;
+	
+	///perform implicit force computation in body space (inertial frame)
+	btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const;
+
+	///explicit version is best avoided, it gains energy
+	btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const;
+	btVector3 getLocalInertia() const;
+
+	///////////////////////////////////////////////
+
+	virtual	int	calculateSerializeBufferSize()	const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+
+	virtual void serializeSingleObject(class btSerializer* serializer) const;
+
+};
+
+//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btRigidBodyFloatData
+{
+	btCollisionObjectFloatData	m_collisionObjectData;
+	btMatrix3x3FloatData		m_invInertiaTensorWorld;
+	btVector3FloatData		m_linearVelocity;
+	btVector3FloatData		m_angularVelocity;
+	btVector3FloatData		m_angularFactor;
+	btVector3FloatData		m_linearFactor;
+	btVector3FloatData		m_gravity;	
+	btVector3FloatData		m_gravity_acceleration;
+	btVector3FloatData		m_invInertiaLocal;
+	btVector3FloatData		m_totalForce;
+	btVector3FloatData		m_totalTorque;
+	float					m_inverseMass;
+	float					m_linearDamping;
+	float					m_angularDamping;
+	float					m_additionalDampingFactor;
+	float					m_additionalLinearDampingThresholdSqr;
+	float					m_additionalAngularDampingThresholdSqr;
+	float					m_additionalAngularDampingFactor;
+	float					m_linearSleepingThreshold;
+	float					m_angularSleepingThreshold;
+	int						m_additionalDamping;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btRigidBodyDoubleData
+{
+	btCollisionObjectDoubleData	m_collisionObjectData;
+	btMatrix3x3DoubleData		m_invInertiaTensorWorld;
+	btVector3DoubleData		m_linearVelocity;
+	btVector3DoubleData		m_angularVelocity;
+	btVector3DoubleData		m_angularFactor;
+	btVector3DoubleData		m_linearFactor;
+	btVector3DoubleData		m_gravity;	
+	btVector3DoubleData		m_gravity_acceleration;
+	btVector3DoubleData		m_invInertiaLocal;
+	btVector3DoubleData		m_totalForce;
+	btVector3DoubleData		m_totalTorque;
+	double					m_inverseMass;
+	double					m_linearDamping;
+	double					m_angularDamping;
+	double					m_additionalDampingFactor;
+	double					m_additionalLinearDampingThresholdSqr;
+	double					m_additionalAngularDampingThresholdSqr;
+	double					m_additionalAngularDampingFactor;
+	double					m_linearSleepingThreshold;
+	double					m_angularSleepingThreshold;
+	int						m_additionalDamping;
+	char	m_padding[4];
+};
+
+
+
+#endif //BT_RIGIDBODY_H
+

lib/haxebullet/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

@@ -0,0 +1,280 @@
+/*
+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 "btSimpleDynamicsWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+
+/*
+  Make sure this dummy function never changes so that it
+  can be used by probes that are checking whether the
+  library is actually installed.
+*/
+extern "C" 
+{
+	void btBulletDynamicsProbe ();
+	void btBulletDynamicsProbe () {}
+}
+
+
+
+
+btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
+m_constraintSolver(constraintSolver),
+m_ownsConstraintSolver(false),
+m_gravity(0,0,-10)
+{
+
+}
+
+
+btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
+{
+	if (m_ownsConstraintSolver)
+		btAlignedFree( m_constraintSolver);
+}
+
+int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+{
+	(void)fixedTimeStep;
+	(void)maxSubSteps;
+
+
+	///apply gravity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	btDispatcherInfo&	dispatchInfo = getDispatchInfo();
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
+	///perform collision detection
+	performDiscreteCollisionDetection();
+
+	///solve contact constraints
+	int numManifolds = m_dispatcher1->getNumManifolds();
+	if (numManifolds)
+	{
+		btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
+		
+		btContactSolverInfo infoGlobal;
+		infoGlobal.m_timeStep = timeStep;
+		m_constraintSolver->prepareSolve(0,numManifolds);
+		m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1);
+		m_constraintSolver->allSolved(infoGlobal,m_debugDrawer);
+	}
+
+	///integrate transforms
+	integrateTransforms(timeStep);
+		
+	updateAabbs();
+
+	synchronizeMotionStates();
+
+	clearForces();
+
+	return 1;
+
+}
+
+void	btSimpleDynamicsWorld::clearForces()
+{
+	///@todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->clearForces();
+		}
+	}
+}	
+
+
+void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
+{
+	m_gravity = gravity;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->setGravity(gravity);
+		}
+	}
+}
+
+btVector3 btSimpleDynamicsWorld::getGravity () const
+{
+	return m_gravity;
+}
+
+void	btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+	btCollisionWorld::removeCollisionObject(body);
+}
+
+void	btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btRigidBody* body = btRigidBody::upcast(collisionObject);
+	if (body)
+		removeRigidBody(body);
+	else
+		btCollisionWorld::removeCollisionObject(collisionObject);
+}
+
+
+void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
+{
+	body->setGravity(m_gravity);
+
+	if (body->getCollisionShape())
+	{
+		addCollisionObject(body);
+	}
+}
+
+void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
+{
+	body->setGravity(m_gravity);
+
+	if (body->getCollisionShape())
+	{
+		addCollisionObject(body,group,mask);
+	}
+}
+
+
+void	btSimpleDynamicsWorld::debugDrawWorld()
+{
+
+}
+				
+void	btSimpleDynamicsWorld::addAction(btActionInterface* action)
+{
+
+}
+
+void	btSimpleDynamicsWorld::removeAction(btActionInterface* action)
+{
+
+}
+
+
+void	btSimpleDynamicsWorld::updateAabbs()
+{
+	btTransform predictedTrans;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			if (body->isActive() && (!body->isStaticObject()))
+			{
+				btVector3 minAabb,maxAabb;
+				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+				btBroadphaseInterface* bp = getBroadphase();
+				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
+			}
+		}
+	}
+}
+
+void	btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	btTransform predictedTrans;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			if (body->isActive() && (!body->isStaticObject()))
+			{
+				body->predictIntegratedTransform(timeStep, predictedTrans);
+				body->proceedToTransform( predictedTrans);
+			}
+		}
+	}
+}
+
+
+
+void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			if (!body->isStaticObject())
+			{
+				if (body->isActive())
+				{
+					body->applyGravity();
+					body->integrateVelocities( timeStep);
+					body->applyDamping(timeStep);
+					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+				}
+			}
+		}
+	}
+}
+
+
+void	btSimpleDynamicsWorld::synchronizeMotionStates()
+{
+	///@todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body && body->getMotionState())
+		{
+			if (body->getActivationState() != ISLAND_SLEEPING)
+			{
+				body->getMotionState()->setWorldTransform(body->getWorldTransform());
+			}
+		}
+	}
+
+}
+
+
+void	btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+	if (m_ownsConstraintSolver)
+	{
+		btAlignedFree(m_constraintSolver);
+	}
+	m_ownsConstraintSolver = false;
+	m_constraintSolver = solver;
+}
+
+btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
+{
+	return m_constraintSolver;
+}

lib/haxebullet/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

@@ -0,0 +1,89 @@
+/*
+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_SIMPLE_DYNAMICS_WORLD_H
+#define BT_SIMPLE_DYNAMICS_WORLD_H
+
+#include "btDynamicsWorld.h"
+
+class btDispatcher;
+class btOverlappingPairCache;
+class btConstraintSolver;
+
+///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
+///Please use btDiscreteDynamicsWorld instead
+class btSimpleDynamicsWorld : public btDynamicsWorld
+{
+protected:
+
+	btConstraintSolver*	m_constraintSolver;
+
+	bool	m_ownsConstraintSolver;
+
+	void	predictUnconstraintMotion(btScalar timeStep);
+	
+	void	integrateTransforms(btScalar timeStep);
+		
+	btVector3	m_gravity;
+	
+public:
+
+
+
+	///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
+	btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btSimpleDynamicsWorld();
+		
+	///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
+	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+
+	virtual void	setGravity(const btVector3& gravity);
+
+	virtual btVector3 getGravity () const;
+
+	virtual void	addRigidBody(btRigidBody* body);
+
+	virtual void	addRigidBody(btRigidBody* body, int group, int mask);
+
+	virtual void	removeRigidBody(btRigidBody* body);
+
+	virtual void	debugDrawWorld();
+				
+	virtual void	addAction(btActionInterface* action);
+
+	virtual void	removeAction(btActionInterface* action);
+
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+	
+	virtual void	updateAabbs();
+
+	virtual void	synchronizeMotionStates();
+
+	virtual void	setConstraintSolver(btConstraintSolver* solver);
+
+	virtual btConstraintSolver* getConstraintSolver();
+
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return BT_SIMPLE_DYNAMICS_WORLD;
+	}
+
+	virtual void	clearForces();
+
+};
+
+#endif //BT_SIMPLE_DYNAMICS_WORLD_H

lib/haxebullet/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp

@@ -0,0 +1,678 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btThreads.h"
+#include "btSimulationIslandManagerMt.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+//#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+
+SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints )
+{
+    // rough estimate of the cost of a batch, used for merging
+    int batchCost = bodies + 8 * manifolds + 4 * constraints;
+    return batchCost;
+}
+
+
+SIMD_FORCE_INLINE int calcBatchCost( const btSimulationIslandManagerMt::Island* island )
+{
+    return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() );
+}
+
+
+btSimulationIslandManagerMt::btSimulationIslandManagerMt()
+{
+    m_minimumSolverBatchSize = calcBatchCost(0, 128, 0);
+    m_batchIslandMinBodyCount = 32;
+    m_islandDispatch = parallelIslandDispatch;
+    m_batchIsland = NULL;
+}
+
+
+btSimulationIslandManagerMt::~btSimulationIslandManagerMt()
+{
+    for ( int i = 0; i < m_allocatedIslands.size(); ++i )
+    {
+        delete m_allocatedIslands[ i ];
+    }
+    m_allocatedIslands.resize( 0 );
+    m_activeIslands.resize( 0 );
+    m_freeIslands.resize( 0 );
+}
+
+
+inline	int	getIslandId(const btPersistentManifold* lhs)
+{
+	const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
+	const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
+    int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
+	return islandId;
+}
+
+
+SIMD_FORCE_INLINE	int	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::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+{
+    BT_PROFILE( "serialIslandDispatch" );
+    // 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
+                                 );
+    }
+}
+
+struct UpdateIslandDispatcher : public btIParallelForBody
+{
+    btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
+    btSimulationIslandManagerMt::IslandCallback* callback;
+
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        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
+            );
+        }
+    }
+};
+
+void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+{
+    BT_PROFILE( "parallelIslandDispatch" );
+    int grainSize = 1;  // iterations per task
+    UpdateIslandDispatcher dispatcher;
+    dispatcher.islandsPtr = islandsPtr;
+    dispatcher.callback = callback;
+    btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
+}
+
+
+///@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 );
+	}
+}

lib/haxebullet/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h

@@ -0,0 +1,110 @@
+/*
+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 "BulletCollision/CollisionDispatch/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 serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
+    static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, 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
+