Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp	(revision 8284)
@@ -44,5 +44,5 @@
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
-#include "LinearMath/btStackAlloc.h"
+
 
 /*
@@ -182,10 +182,10 @@
 {
 	//capsule is convex hull of 2 spheres, so use btMultiSphereShape
-	btVector3 inertiaHalfExtents(radius,height,radius);
+	
 	const int numSpheres = 2;
 	btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
 	btScalar radi[numSpheres] = {radius,radius};
 	void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
-	return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres);
+	return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
 }
 plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
@@ -294,4 +294,12 @@
 	worldTrans.setRotation(orn);
 	body->setWorldTransform(worldTrans);
+}
+
+void	plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	btTransform& worldTrans = body->getWorldTransform();
+	worldTrans.setFromOpenGLMatrix(matrix);
 }
 
@@ -366,8 +374,5 @@
 	btGjkPairDetector::ClosestPointInput input;
 	
-	btStackAlloc gStackAlloc(1024*1024*2);
- 
-	input.m_stackAlloc = &gStackAlloc;
-	
+		
 	btTransform tr;
 	tr.setIdentity();
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btActionInterface.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btActionInterface.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btActionInterface.h	(revision 8284)
@@ -21,9 +21,15 @@
 
 #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
 {
-	public:
+protected:
+
+	static btRigidBody& getFixedBody();
+	
+	
+public:
 
 	virtual ~btActionInterface()
@@ -38,2 +44,3 @@
 
 #endif //_BT_ACTION_INTERFACE_H
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp	(revision 8284)
@@ -49,4 +49,8 @@
 	startProfiling(timeStep);
 	
+	if(0 != m_internalPreTickCallback) {
+		(*m_internalPreTickCallback)(this, timeStep);
+	}
+
 
 	///update aabbs information
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp	(revision 8284)
@@ -1,5 +1,5 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
@@ -20,4 +20,5 @@
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
@@ -36,18 +37,6 @@
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 
-//for debug rendering
-#include "BulletCollision/CollisionShapes/btBoxShape.h"
-#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
-#include "BulletCollision/CollisionShapes/btCompoundShape.h"
-#include "BulletCollision/CollisionShapes/btConeShape.h"
-#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
-#include "BulletCollision/CollisionShapes/btCylinderShape.h"
-#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
-#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
-#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
-#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
-#include "LinearMath/btIDebugDraw.h"
 
 
@@ -56,5 +45,5 @@
 #include "LinearMath/btMotionState.h"
 
-
+#include "LinearMath/btSerializer.h"
 
 
@@ -64,5 +53,6 @@
 m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
-m_localTime(btScalar(1.)/btScalar(60.)),
+m_localTime(0),
+m_synchronizeAllMotionStates(false),
 m_profileTimings(0)
 {
@@ -104,21 +94,21 @@
 void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 {
-
+///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+///to switch status _after_ adding kinematic objects to the world
+///fix it for Bullet 3.x release
 	for (int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
-		{
-				if (body->getActivationState() != ISLAND_SLEEPING)
-				{
-					if (body->isKinematicObject())
-					{
-						//to calculate velocities next frame
-						body->saveKinematicState(timeStep);
-					}
-				}
-		}
-	}
+		if (body && body->getActivationState() != ISLAND_SLEEPING)
+		{
+			if (body->isKinematicObject())
+			{
+				//to calculate velocities next frame
+				body->saveKinematicState(timeStep);
+			}
+		}
+	}
+
 }
 
@@ -127,22 +117,6 @@
 	BT_PROFILE("debugDrawWorld");
 
-	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
-	{
-		int numManifolds = getDispatcher()->getNumManifolds();
-		btVector3 color(0,0,0);
-		for (int i=0;i<numManifolds;i++)
-		{
-			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
-			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
-			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
-
-			int numContacts = contactManifold->getNumContacts();
-			for (int j=0;j<numContacts;j++)
-			{
-				btManifoldPoint& cp = contactManifold->getContactPoint(j);
-				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
-			}
-		}
-	}
+	btCollisionWorld::debugDrawWorld();
+
 	bool drawConstraints = false;
 	if (getDebugDrawer())
@@ -169,40 +143,4 @@
 		int i;
 
-		for (  i=0;i<m_collisionObjects.size();i++)
-		{
-			btCollisionObject* colObj = m_collisionObjects[i];
-			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
-			{
-				btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
-				switch(colObj->getActivationState())
-				{
-				case  ACTIVE_TAG:
-					color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
-				case ISLAND_SLEEPING:
-					color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
-				case WANTS_DEACTIVATION:
-					color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
-				case DISABLE_DEACTIVATION:
-					color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
-				case DISABLE_SIMULATION:
-					color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
-				default:
-					{
-						color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
-					}
-				};
-
-				debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
-			}
-			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
-			{
-				btVector3 minAabb,maxAabb;
-				btVector3 colorvec(1,0,0);
-				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-				m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
-			}
-
-		}
-	
 		if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
 		{
@@ -218,13 +156,10 @@
 {
 	///@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();
-		}
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		//need to check if next line is ok
+		//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
+		body->clearForces();
 	}
 }	
@@ -234,10 +169,8 @@
 {
 	///@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->isActive())
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		if (body->isActive())
 		{
 			body->applyGravity();
@@ -270,30 +203,24 @@
 {
 	BT_PROFILE("synchronizeMotionStates");
-	{
-		//todo: iterate over awake simulation islands!
+	if (m_synchronizeAllMotionStates)
+	{
+		//iterate  over all collision objects
 		for ( int i=0;i<m_collisionObjects.size();i++)
 		{
 			btCollisionObject* colObj = m_collisionObjects[i];
-			
 			btRigidBody* body = btRigidBody::upcast(colObj);
 			if (body)
 				synchronizeSingleMotionState(body);
 		}
-	}
-/*
-	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
-	{
-		for ( int i=0;i<this->m_vehicles.size();i++)
-		{
-			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
-			{
-				//synchronize the wheels with the (interpolated) chassis worldtransform
-				m_vehicles[i]->updateWheelTransform(v,true);
-			}
-		}
-	}
-	*/
-
-
+	} else
+	{
+		//iterate over all active rigid bodies
+		for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+		{
+			btRigidBody* body = m_nonStaticRigidBodies[i];
+			if (body->isActive())
+				synchronizeSingleMotionState(body);
+		}
+	}
 }
 
@@ -341,11 +268,13 @@
 	{
 
-		saveKinematicState(fixedTimeStep);
-
-		applyGravity();
-
 		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
 		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
 
+		saveKinematicState(fixedTimeStep*clampedSimulationSteps);
+
+		applyGravity();
+
+		
+
 		for (int i=0;i<clampedSimulationSteps;i++)
 		{
@@ -354,7 +283,8 @@
 		}
 
-	} 
-
-	synchronizeMotionStates();
+	} else
+	{
+		synchronizeMotionStates();
+	}
 
 	clearForces();
@@ -372,4 +302,8 @@
 	BT_PROFILE("internalSingleStepSimulation");
 
+	if(0 != m_internalPreTickCallback) {
+		(*m_internalPreTickCallback)(this, timeStep);
+	}	
+
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
@@ -412,9 +346,8 @@
 {
 	m_gravity = gravity;
-	for ( int i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
 		{
 			body->setGravity(gravity);
@@ -428,13 +361,28 @@
 }
 
+void	btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+{
+	btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
+}
+
+void	btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btRigidBody* body = btRigidBody::upcast(collisionObject);
+	if (body)
+		removeRigidBody(body);
+	else
+		btCollisionWorld::removeCollisionObject(collisionObject);
+}
 
 void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
-	removeCollisionObject(body);
-}
+	m_nonStaticRigidBodies.remove(body);
+	btCollisionWorld::removeCollisionObject(body);
+}
+
 
 void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 {
-	if (!body->isStaticOrKinematicObject())
+	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
 	{
 		body->setGravity(m_gravity);
@@ -443,4 +391,12 @@
 	if (body->getCollisionShape())
 	{
+		if (!body->isStaticObject())
+		{
+			m_nonStaticRigidBodies.push_back(body);
+		} else
+		{
+			body->setActivationState(ISLAND_SLEEPING);
+		}
+
 		bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
 		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
@@ -453,5 +409,5 @@
 void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
 {
-	if (!body->isStaticOrKinematicObject())
+	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
 	{
 		body->setGravity(m_gravity);
@@ -460,4 +416,12 @@
 	if (body->getCollisionShape())
 	{
+		if (!body->isStaticObject())
+		{
+			m_nonStaticRigidBodies.push_back(body);
+		}
+		 else
+		{
+			body->setActivationState(ISLAND_SLEEPING);
+		}
 		addCollisionObject(body,group,mask);
 	}
@@ -480,8 +444,7 @@
 	BT_PROFILE("updateActivationState");
 
-	for ( int i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (body)
 		{
@@ -586,5 +549,4 @@
 		}
 };
-
 
 
@@ -604,4 +566,9 @@
 		btStackAlloc*			m_stackAlloc;
 		btDispatcher*			m_dispatcher;
+		
+		btAlignedObjectArray<btCollisionObject*> m_bodies;
+		btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+		btAlignedObjectArray<btTypedConstraint*> m_constraints;
+
 
 		InplaceSolverIslandCallback(
@@ -624,4 +591,5 @@
 		}
 
+
 		InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
 		{
@@ -664,14 +632,45 @@
 				}
 
-				///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
-				if (numManifolds + numCurConstraints)
-				{
-					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
-				}
-		
-			}
+				if (m_solverInfo.m_minimumSolverBatchSize<=1)
+				{
+					///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+					if (numManifolds + numCurConstraints)
+					{
+						m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+					}
+				} else
+				{
+					
+					for (i=0;i<numBodies;i++)
+						m_bodies.push_back(bodies[i]);
+					for (i=0;i<numManifolds;i++)
+						m_manifolds.push_back(manifolds[i]);
+					for (i=0;i<numCurConstraints;i++)
+						m_constraints.push_back(startConstraint[i]);
+					if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize)
+					{
+						processConstraints();
+					} else
+					{
+						//printf("deferred\n");
+					}
+				}
+			}
+		}
+		void	processConstraints()
+		{
+			if (m_manifolds.size() + m_constraints.size()>0)
+			{
+				m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+			}
+			m_bodies.resize(0);
+			m_manifolds.resize(0);
+			m_constraints.resize(0);
+
 		}
 
 	};
+
+	
 
 	//sorted version of all btTypedConstraint, based on islandId
@@ -699,4 +698,6 @@
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
 
+	solverCallback.processConstraints();
+
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
 }
@@ -741,5 +742,5 @@
 
 
-#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
 
 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
@@ -754,6 +755,6 @@
 	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
 	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
+		m_me(me),
 		m_allowedPenetration(0.0f),
-		m_me(me),
 		m_pairCache(pairCache),
 		m_dispatcher(dispatcher)
@@ -829,44 +830,40 @@
 	BT_PROFILE("integrateTransforms");
 	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
-		{
-			body->setHitFraction(1.f);
-
-			if (body->isActive() && (!body->isStaticOrKinematicObject()))
-			{
-				body->predictIntegratedTransform(timeStep, predictedTrans);
-				btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
-
-				if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
-				{
-					BT_PROFILE("CCD motion clamping");
-					if (body->getCollisionShape()->isConvex())
-					{
-						gNumClampedCcdMotions++;
-						
-						btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
-						btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-						btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-
-						sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
-						sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
-
-						convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
-						if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
-						{
-							body->setHitFraction(sweepResults.m_closestHitFraction);
-							body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
-							body->setHitFraction(0.f);
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		body->setHitFraction(1.f);
+
+		if (body->isActive() && (!body->isStaticOrKinematicObject()))
+		{
+			body->predictIntegratedTransform(timeStep, predictedTrans);
+			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+
+			if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
+			{
+				BT_PROFILE("CCD motion clamping");
+				if (body->getCollisionShape()->isConvex())
+				{
+					gNumClampedCcdMotions++;
+					
+					btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+
+					sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
+					sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+
+					convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
+					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
+					{
+						body->setHitFraction(sweepResults.m_closestHitFraction);
+						body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+						body->setHitFraction(0.f);
 //							printf("clamped integration to hit fraction = %f\n",fraction);
-						}
 					}
 				}
-				
-				body->proceedToTransform( predictedTrans);
-			}
+			}
+			
+			body->proceedToTransform( predictedTrans);
 		}
 	}
@@ -880,19 +877,14 @@
 {
 	BT_PROFILE("predictUnconstraintMotion");
-	for ( int i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
-		{
-			if (!body->isStaticOrKinematicObject())
-			{
-				
-				body->integrateVelocities( timeStep);
-				//damping
-				body->applyDamping(timeStep);
-
-				body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
-			}
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		if (!body->isStaticOrKinematicObject())
+		{
+			body->integrateVelocities( timeStep);
+			//damping
+			body->applyDamping(timeStep);
+
+			body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
 		}
 	}
@@ -914,279 +906,4 @@
 
 	
-
-class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
-{
-	btIDebugDraw*	m_debugDrawer;
-	btVector3	m_color;
-	btTransform	m_worldTrans;
-
-public:
-
-	DebugDrawcallback(btIDebugDraw*	debugDrawer,const btTransform& worldTrans,const btVector3& color) :
-                m_debugDrawer(debugDrawer),
-		m_color(color),
-		m_worldTrans(worldTrans)
-	{
-	}
-
-	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
-	{
-		processTriangle(triangle,partId,triangleIndex);
-	}
-
-	virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
-	{
-		(void)partId;
-		(void)triangleIndex;
-
-		btVector3 wv0,wv1,wv2;
-		wv0 = m_worldTrans*triangle[0];
-		wv1 = m_worldTrans*triangle[1];
-		wv2 = m_worldTrans*triangle[2];
-		m_debugDrawer->drawLine(wv0,wv1,m_color);
-		m_debugDrawer->drawLine(wv1,wv2,m_color);
-		m_debugDrawer->drawLine(wv2,wv0,m_color);
-	}
-};
-
-void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
-{
-	btVector3 start = transform.getOrigin();
-
-	const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
-	const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
-	const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
-
-	// XY 
-	getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
-	getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
-	getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
-	getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
-
-	// XZ
-	getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
-	getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
-	getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
-	getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
-
-	// YZ
-	getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
-	getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
-	getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
-	getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
-}
-
-void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
-{
-	// Draw a small simplex at the center of the object
-	{
-		btVector3 start = worldTransform.getOrigin();
-		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
-		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
-		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
-	}
-
-	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
-	{
-		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
-		for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
-		{
-			btTransform childTrans = compoundShape->getChildTransform(i);
-			const btCollisionShape* colShape = compoundShape->getChildShape(i);
-			debugDrawObject(worldTransform*childTrans,colShape,color);
-		}
-
-	} else
-	{
-		switch (shape->getShapeType())
-		{
-
-		case SPHERE_SHAPE_PROXYTYPE:
-			{
-				const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
-				btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
-				
-				debugDrawSphere(radius, worldTransform, color);
-				break;
-			}
-		case MULTI_SPHERE_SHAPE_PROXYTYPE:
-			{
-				const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
-
-				for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
-				{
-					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
-					debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
-				}
-
-				break;
-			}
-		case CAPSULE_SHAPE_PROXYTYPE:
-			{
-				const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
-
-				btScalar radius = capsuleShape->getRadius();
-				btScalar halfHeight = capsuleShape->getHalfHeight();
-				
-				int upAxis = capsuleShape->getUpAxis();
-
-				
-				btVector3 capStart(0.f,0.f,0.f);
-				capStart[upAxis] = -halfHeight;
-
-				btVector3 capEnd(0.f,0.f,0.f);
-				capEnd[upAxis] = halfHeight;
-
-				// Draw the ends
-				{
-					
-					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * capStart;
-					debugDrawSphere(radius, childTransform, color);
-				}
-
-				{
-					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * capEnd;
-					debugDrawSphere(radius, childTransform, color);
-				}
-
-				// Draw some additional lines
-				btVector3 start = worldTransform.getOrigin();
-
-				
-				capStart[(upAxis+1)%3] = radius;
-				capEnd[(upAxis+1)%3] = radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-				capStart[(upAxis+1)%3] = -radius;
-				capEnd[(upAxis+1)%3] = -radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-
-				capStart[(upAxis+1)%3] = 0.f;
-				capEnd[(upAxis+1)%3] = 0.f;
-
-				capStart[(upAxis+2)%3] = radius;
-				capEnd[(upAxis+2)%3] = radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-				capStart[(upAxis+2)%3] = -radius;
-				capEnd[(upAxis+2)%3] = -radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-
-				
-				break;
-			}
-		case CONE_SHAPE_PROXYTYPE:
-			{
-				const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
-				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
-				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
-				btVector3 start = worldTransform.getOrigin();
-
-				int upAxis= coneShape->getConeUpIndex();
-				
-
-				btVector3	offsetHeight(0,0,0);
-				offsetHeight[upAxis] = height * btScalar(0.5);
-				btVector3	offsetRadius(0,0,0);
-				offsetRadius[(upAxis+1)%3] = radius;
-				btVector3	offset2Radius(0,0,0);
-				offset2Radius[(upAxis+2)%3] = radius;
-
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
-
-
-
-				break;
-
-			}
-		case CYLINDER_SHAPE_PROXYTYPE:
-			{
-				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
-				int upAxis = cylinder->getUpAxis();
-				btScalar radius = cylinder->getRadius();
-				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
-				btVector3 start = worldTransform.getOrigin();
-				btVector3	offsetHeight(0,0,0);
-				offsetHeight[upAxis] = halfHeight;
-				btVector3	offsetRadius(0,0,0);
-				offsetRadius[(upAxis+1)%3] = radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-				break;
-			}
-
-			case STATIC_PLANE_PROXYTYPE:
-				{
-					const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
-					btScalar planeConst = staticPlaneShape->getPlaneConstant();
-					const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
-					btVector3 planeOrigin = planeNormal * planeConst;
-					btVector3 vec0,vec1;
-					btPlaneSpace1(planeNormal,vec0,vec1);
-					btScalar vecLen = 100.f;
-					btVector3 pt0 = planeOrigin + vec0*vecLen;
-					btVector3 pt1 = planeOrigin - vec0*vecLen;
-					btVector3 pt2 = planeOrigin + vec1*vecLen;
-					btVector3 pt3 = planeOrigin - vec1*vecLen;
-					getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
-					getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
-					break;
-
-				}
-		default:
-			{
-
-				if (shape->isConcave())
-				{
-					btConcaveShape* concaveMesh = (btConcaveShape*) shape;
-					
-					///@todo pass camera, for some culling? no -> we are not a graphics lib
-					btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-					btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-
-					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-					concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
-
-				}
-
-				if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
-				{
-					btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
-					//todo: pass camera for some culling			
-					btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-					btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-					//DebugDrawcallback drawCallback;
-					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-					convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
-				}
-
-
-				/// for polyhedral shapes
-				if (shape->isPolyhedral())
-				{
-					btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
-
-					int i;
-					for (i=0;i<polyshape->getNumEdges();i++)
-					{
-						btVector3 a,b;
-						polyshape->getEdge(i,a,b);
-						btVector3 wa = worldTransform * a;
-						btVector3 wb = worldTransform * b;
-						getDebugDrawer()->drawLine(wa,wb,color);
-
-					}
-
-					
-				}
-			}
-		}
-	}
-}
-
 
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
@@ -1350,5 +1067,5 @@
 				if(drawLimits)
 				{
-					btTransform tr = pSlider->getCalculatedTransformA();
+					btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
 					btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
 					btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
@@ -1367,5 +1084,5 @@
 	}
 	return;
-} // btDiscreteDynamicsWorld::debugDrawConstraint()
+}
 
 
@@ -1403,2 +1120,42 @@
 
 
+
+void	btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
+{
+	int i;
+	//serialize all collision objects
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len,1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
+		}
+	}
+
+	for (i=0;i<m_constraints.size();i++)
+	{
+		btTypedConstraint* constraint = m_constraints[i];
+		int size = constraint->calculateSerializeBufferSize();
+		btChunk* chunk = serializer->allocate(size,1);
+		const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
+		serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
+	}
+}
+
+
+void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
+{
+
+	serializer->startSerialization();
+
+	serializeRigidBodies(serializer);
+
+	serializeCollisionObjects(serializer);
+
+	serializer->finishSerialization();
+}
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h	(revision 8284)
@@ -1,5 +1,5 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
@@ -13,4 +13,5 @@
 3. This notice may not be removed or altered from any source distribution.
 */
+
 
 #ifndef BT_DISCRETE_DYNAMICS_WORLD_H
@@ -42,4 +43,6 @@
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
+	btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
+
 	btVector3	m_gravity;
 
@@ -50,4 +53,5 @@
 	bool	m_ownsIslandManager;
 	bool	m_ownsConstraintSolver;
+	bool	m_synchronizeAllMotionStates;
 
 	btAlignedObjectArray<btActionInterface*>	m_actions;
@@ -74,6 +78,5 @@
 	virtual void	saveKinematicState(btScalar timeStep);
 
-	void	debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
-
+	void	serializeRigidBodies(btSerializer* serializer);
 
 public:
@@ -121,4 +124,6 @@
 	virtual btVector3 getGravity () const;
 
+	virtual void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
 	virtual void	addRigidBody(btRigidBody* body);
 
@@ -127,5 +132,7 @@
 	virtual void	removeRigidBody(btRigidBody* body);
 
-	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+
 
 	void	debugDrawConstraint(btTypedConstraint* constraint);
@@ -175,4 +182,16 @@
 	virtual void	removeCharacter(btActionInterface* character);
 
+	void	setSynchronizeAllMotionStates(bool synchronizeAll)
+	{
+		m_synchronizeAllMotionStates = synchronizeAll;
+	}
+	bool getSynchronizeAllMotionStates() const
+	{
+		return m_synchronizeAllMotionStates;
+	}
+
+	///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
+	virtual	void	serialize(btSerializer* serializer);
+
 };
 
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h	(revision 8284)
@@ -42,4 +42,5 @@
 protected:
 		btInternalTickCallback m_internalTickCallback;
+		btInternalTickCallback m_internalPreTickCallback;
 		void*	m_worldUserInfo;
 
@@ -50,5 +51,5 @@
 
 		btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
-		:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
+		:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
 		{
 		}
@@ -103,7 +104,13 @@
 
 		/// Set the callback for when an internal tick (simulation substep) happens, optional user info
-		void setInternalTickCallback(btInternalTickCallback cb,	void* worldUserInfo=0) 
+		void setInternalTickCallback(btInternalTickCallback cb,	void* worldUserInfo=0,bool isPreTick=false) 
 		{ 
-			m_internalTickCallback = cb; 
+			if (isPreTick)
+			{
+				m_internalPreTickCallback = cb;
+			} else
+			{
+				m_internalTickCallback = cb; 
+			}
 			m_worldUserInfo = worldUserInfo;
 		}
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp	(revision 8284)
@@ -20,4 +20,5 @@
 #include "LinearMath/btMotionState.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "LinearMath/btSerializer.h"
 
 //'temporarily' global variables
@@ -45,5 +46,6 @@
 	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
-	m_angularFactor = btScalar(1.);
+	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));
@@ -86,4 +88,15 @@
 	updateInertiaTensor();
 
+	m_rigidbodyFlags = 0;
+
+
+	m_deltaLinearVelocity.setZero();
+	m_deltaAngularVelocity.setZero();
+	m_invMass = m_inverseMass*m_linearFactor;
+	m_pushVelocity.setZero();
+	m_turnVelocity.setZero();
+
+	
+
 }
 
@@ -136,6 +149,6 @@
 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 {
-	m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	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));
 }
 
@@ -227,4 +240,7 @@
 		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),
@@ -232,4 +248,5 @@
 				   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
 
+	m_invMass = m_linearFactor*m_inverseMass;
 }
 
@@ -301,4 +318,26 @@
 }
 
+void	btRigidBody::internalWritebackVelocity(btScalar timeStep)
+{
+    (void) timeStep;
+	if (m_inverseMass)
+	{
+		setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
+		setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
+		
+		//correct the position/orientation based on push/turn recovery
+		btTransform newTransform;
+		btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+		setWorldTransform(newTransform);
+		//m_originalBody->setCompanionId(-1);
+	}
+//	m_deltaLinearVelocity.setZero();
+//	m_deltaAngularVelocity .setZero();
+//	m_pushVelocity.setZero();
+//	m_turnVelocity.setZero();
+}
+
+
+
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
 {
@@ -315,2 +354,50 @@
 	m_checkCollideWith = m_constraintRefs.size() > 0;
 }
+
+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;
+
+	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);
+}
+
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.h	(revision 8284)
@@ -29,4 +29,18 @@
 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
+};
 
 
@@ -46,5 +60,5 @@
 	btVector3		m_angularVelocity;
 	btScalar		m_inverseMass;
-	btScalar		m_angularFactor;
+	btVector3		m_linearFactor;
 
 	btVector3		m_gravity;	
@@ -72,4 +86,19 @@
 	//keep track of typed constraints referencing this rigid body
 	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
+
+	int				m_rigidbodyFlags;
+	
+	int				m_debugBodyId;
+
+
+protected:
+
+	ATTRIBUTE_ALIGNED64(btVector3		m_deltaLinearVelocity);
+	btVector3		m_deltaAngularVelocity;
+	btVector3		m_angularFactor;
+	btVector3		m_invMass;
+	btVector3		m_pushVelocity;
+	btVector3		m_turnVelocity;
+
 
 public:
@@ -111,5 +140,4 @@
 		btScalar			m_additionalAngularDampingFactor;
 
-		
 		btRigidBodyConstructionInfo(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
 		m_mass(mass),
@@ -161,5 +189,5 @@
 	static const btRigidBody*	upcast(const btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+		if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
 			return (const btRigidBody*)colObj;
 		return 0;
@@ -167,5 +195,5 @@
 	static btRigidBody*	upcast(btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+		if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
 			return (btRigidBody*)colObj;
 		return 0;
@@ -220,4 +248,13 @@
 	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 { 
@@ -231,5 +268,5 @@
 	void			applyCentralForce(const btVector3& force)
 	{
-		m_totalForce += force;
+		m_totalForce += force*m_linearFactor;
 	}
 
@@ -262,5 +299,5 @@
 	void	applyTorque(const btVector3& torque)
 	{
-		m_totalTorque += torque;
+		m_totalTorque += torque*m_angularFactor;
 	}
 	
@@ -268,15 +305,15 @@
 	{
 		applyCentralForce(force);
-		applyTorque(rel_pos.cross(force)*m_angularFactor);
+		applyTorque(rel_pos.cross(force*m_linearFactor));
 	}
 	
 	void applyCentralImpulse(const btVector3& impulse)
 	{
-		m_linearVelocity += impulse * m_inverseMass;
+		m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
 	}
 	
   	void applyTorqueImpulse(const btVector3& torque)
 	{
-			m_angularVelocity += m_invInertiaTensorWorld * torque;
+			m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
 	}
 	
@@ -288,22 +325,9 @@
 			if (m_angularFactor)
 			{
-				applyTorqueImpulse(rel_pos.cross(impulse)*m_angularFactor);
+				applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
 			}
 		}
 	}
 
-	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-	{
-		if (m_inverseMass != btScalar(0.))
-		{
-			m_linearVelocity += linearComponent*impulseMagnitude;
-			if (m_angularFactor)
-			{
-				m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
-			}
-		}
-	}
-	
 	void clearForces() 
 	{
@@ -451,9 +475,14 @@
 	int	m_frictionSolverType;
 
+	void	setAngularFactor(const btVector3& angFac)
+	{
+		m_angularFactor = angFac;
+	}
+
 	void	setAngularFactor(btScalar angFac)
 	{
-		m_angularFactor = angFac;
-	}
-	btScalar	getAngularFactor() const
+		m_angularFactor.setValue(angFac,angFac,angFac);
+	}
+	const btVector3&	getAngularFactor() const
 	{
 		return m_angularFactor;
@@ -481,7 +510,179 @@
 	}
 
-	int	m_debugBodyId;
+	void	setFlags(int flags)
+	{
+		m_rigidbodyFlags = flags;
+	}
+
+	int getFlags() const
+	{
+		return m_rigidbodyFlags;
+	}
+
+	const btVector3& getDeltaLinearVelocity() const
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	const btVector3& getDeltaAngularVelocity() const
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const btVector3& getPushVelocity() const 
+	{
+		return m_pushVelocity;
+	}
+
+	const btVector3& getTurnVelocity() const 
+	{
+		return m_turnVelocity;
+	}
+
+
+	////////////////////////////////////////////////
+	///some internal methods, don't use them
+		
+	btVector3& internalGetDeltaLinearVelocity()
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	btVector3& internalGetDeltaAngularVelocity()
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const btVector3& internalGetAngularFactor() const
+	{
+		return m_angularFactor;
+	}
+
+	const btVector3& internalGetInvMass() const
+	{
+		return m_invMass;
+	}
+	
+	btVector3& internalGetPushVelocity()
+	{
+		return m_pushVelocity;
+	}
+
+	btVector3& internalGetTurnVelocity()
+	{
+		return m_turnVelocity;
+	}
+
+	SIMD_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+	{
+		velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+	}
+
+	SIMD_FORCE_INLINE void	internalGetAngularVelocity(btVector3& angVel) const
+	{
+		angVel = getAngularVelocity()+m_deltaAngularVelocity;
+	}
+
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+	{
+		if (m_inverseMass)
+		{
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		if (m_inverseMass)
+		{
+			m_pushVelocity += linearComponent*impulseMagnitude;
+			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+	
+	void	internalWritebackVelocity()
+	{
+		if (m_inverseMass)
+		{
+			setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
+			setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
+			//m_deltaLinearVelocity.setZero();
+			//m_deltaAngularVelocity .setZero();
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+
+
+	void	internalWritebackVelocity(btScalar timeStep);
+	
+
+	///////////////////////////////////////////////
+
+	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];
+};
+
 
 
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp	(revision 8284)
@@ -133,6 +133,16 @@
 void	btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
-	removeCollisionObject(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)
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h	(revision 8284)
@@ -58,4 +58,7 @@
 
 	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	updateAabbs();
