processed a lot of feedback: added 'realtime' simulation with fixed substeps (and clamping maximum number of substeps), this means that when stepSimulation is called with smaller timesteps then 'fixed substep' the motionstate is interpolated.

renamed m_ccdSweptSphereRadius,
enabled wireframe debugDrawObject (using debugDrawer)

Demos/BasicDemo/BasicDemo.cpp

@@ -38,7 +38,7 @@ int main(int argc,char** argv)
 
 	BasicDemo ccdDemo;
 	ccdDemo.initPhysics();
-	ccdDemo.setCameraDistance(50.f);
+	ccdDemo.setCameraDistance(10.f);
 
 #ifdef CHECK_MEMORY_LEAKS
 	ccdDemo.exitPhysics();
@@ -60,8 +60,11 @@ void BasicDemo::clientMoveAndDisplay()
 {
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 
+	//simple dynamics world doesn't handle fixed-time-stepping
+	float ms = m_clock.getTimeMilliseconds();
+	m_clock.reset();
 	if (m_dynamicsWorld)
-		m_dynamicsWorld->stepSimulation(deltaTime);
+		m_dynamicsWorld->stepSimulation(ms / 1000.f);
 		
 	renderme(); 
 
@@ -95,13 +98,6 @@ void BasicDemo::displayCallback(void) {
 
 
 
-///make this positive to show stack falling from a distance
-///this shows the penalty tresholds in action, springy/spungy look
-
-void BasicDemo::clientResetScene()
-{
-}
-
 
 void	BasicDemo::initPhysics()
 {
@@ -164,7 +160,7 @@ void	BasicDemo::initPhysics()
 		btTransform trans;
 		trans.setIdentity();
 		//stack them
-		int colsize = 10;
+		int colsize = 2;
 		int row = (i*HALF_EXTENTS*2)/(colsize*2*HALF_EXTENTS);
 		int row2 = row;
 		int col = (i)%(colsize)-colsize/2;

Demos/BasicDemo/BasicDemo.h

@@ -52,7 +52,6 @@ class BasicDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene();
 	
 };
 

Demos/BspDemo/BspDemo.h

@@ -30,9 +30,7 @@ class BspDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
+	
-	{
-	}
 
 };
 

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -25,7 +25,7 @@ subject to the following restrictions:
 
 #include "btBulletDynamicsCommon.h"
 
-#ifdef COMPARE_WITH_QUICKSTEP 1
+#ifdef COMPARE_WITH_QUICKSTEP
 #include "../Extras/quickstep/OdeConstraintSolver.h"
 #endif //COMPARE_WITH_QUICKSTEP
 
@@ -114,6 +114,7 @@ GLDebugDrawer debugDrawer;
 int main(int argc,char** argv)
 {
 
+
 	CcdPhysicsDemo* ccdDemo = new CcdPhysicsDemo();
 
 	ccdDemo->initPhysics();
@@ -141,8 +142,32 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
 	m_dynamicsWorld->getCollisionObjectArray()[0]->m_worldTransform.getOrigin() += kinTranslation;
 #endif //USE_KINEMATIC_GROUND
 
+	float dt = m_clock.getTimeMilliseconds() * 0.001f;
+	m_clock.reset();
+	printf("dt = %f: ",dt);
+	
 	if (m_dynamicsWorld)
-		m_dynamicsWorld->stepSimulation(deltaTime);
+	{
+		//during idle mode, just run 1 simulation step maximum
+		int maxSimSubSteps = m_idle ? 1 : 1;
+		if (m_idle)
+			dt = 1.0/420.f;
+
+		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
+		if (!numSimSteps)
+			printf("Interpolated transforms\n");
+		else
+		{
+			if (numSimSteps > maxSimSubSteps)
+			{
+				//detect dropping frames
+				printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
+			} else
+			{
+				printf("Simulated (%i) steps\n",numSimSteps);
+			}
+		}
+	}
 	
 #ifdef USE_QUICKPROF 
         btProfiler::beginBlock("render"); 
@@ -183,59 +208,6 @@ void CcdPhysicsDemo::displayCallback(void) {
 
 
 
-///make this positive to show stack falling from a distance
-///this shows the penalty tresholds in action, springy/spungy look
-
-void CcdPhysicsDemo::clientResetScene()
-{
-
-/*	
-	int i;
-	int numObjects = m_physicsEnvironmentPtr->GetNumControllers();
-
-	for (i=0;i<numObjects;i++)
-	{
-		//skip the first object (static ground)
-		if (i>0)
-		{
-			CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i);
-
-			if ((getDebugMode() & btIDebugDraw::DBG_NoHelpText))
-			{
-				if (ctrl->getRigidBody()->getCollisionShape()->getShapeType() != SPHERE_SHAPE_PROXYTYPE)
-				{
-					ctrl->getRigidBody()->SetCollisionShape(shapePtr[2]);
-				} else
-				{
-					ctrl->getRigidBody()->SetCollisionShape(shapePtr[1]);
-				}
-
-				btBroadphaseProxy* bpproxy = ctrl->getRigidBody()->m_broadphaseHandle;
-				m_physicsEnvironmentPtr->getBroadphase()->cleanProxyFromPairs(bpproxy);
-			}
-
-			//stack them
-			int colsize = 10;
-			int row = (i*CUBE_HALF_EXTENTS*2)/(colsize*2*CUBE_HALF_EXTENTS);
-			int row2 = row;
-			int col = (i)%(colsize)-colsize/2;
-
-
-			if (col>3)
-			{
-				col=11;
-				row2 |=1;
-			}
-			ctrl->setPosition(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS,
-				row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0);
-			ctrl->setOrientation(0,0,0,1);
-			ctrl->SetLinearVelocity(0,0,0,false);
-			ctrl->SetAngularVelocity(0,0,0,false);
-		} 
-	}
-	*/
-
-}
 
 
 ///User-defined friction model, the most simple friction model available: no friction
@@ -368,7 +340,7 @@ void	CcdPhysicsDemo::initPhysics()
 		body->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS;
 		
 		//Experimental: better estimation of CCD Time of Impact:
-		body->m_ccdSweptShereRadius = 0.2*CUBE_HALF_EXTENTS;
+		body->m_ccdSweptSphereRadius = 0.2*CUBE_HALF_EXTENTS;
 
 #ifdef USER_DEFINED_FRICTION_MODEL	
 		///Advanced use: override the friction solver

Demos/CcdPhysicsDemo/CcdPhysicsDemo.h

@@ -28,7 +28,6 @@ class CcdPhysicsDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene();
 	
 };
 

Demos/ColladaDemo/ColladaDemo.h

@@ -29,9 +29,6 @@ class ColladaDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
-	{
-	}
 
 	virtual void keyboardCallback(unsigned char key, int x, int y);
 

Demos/CollisionDemo/CollisionDemo.h

@@ -28,9 +28,6 @@ class CollisionDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
-	{
-	}
 };
 
 #endif //COLLISION_DEMO_H

Demos/ConcaveDemo/ConcaveDemo.h

@@ -29,7 +29,6 @@ class ConcaveDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene();
 	
 };
 

Demos/ConcaveDemo/ConcavePhysicsDemo.cpp

@@ -192,23 +192,6 @@ void ConcaveDemo::clientMoveAndDisplay()
 
 }
 
-void	ConcaveDemo::clientResetScene()
-{
-	/*int numObj = m_physicsEnvironmentPtr->GetNumControllers();
-
-	//skip ground
-	for (int i=1;i<numObj;i++)
-	{
-		CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i);
-		ctrl->setPosition(2*i,1,1);
-		ctrl->setOrientation(0,0,0,1);
-		ctrl->SetLinearVelocity(0,0,0,0);
-		ctrl->SetAngularVelocity(0,0,0,0);
-	}
-	*/
-
-}
-
 
 
 

Demos/ConstraintDemo/ConstraintDemo.h

@@ -28,9 +28,7 @@ class ConstraintDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
+	
-	{
-	}
 };
 
 #endif //CONSTRAINT_DEMO_H

Demos/ContinuousConvexCollision/ContinuousConvexCollision.h

@@ -28,9 +28,7 @@ class btContinuousConvexCollisionDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
+	
-	{
-	}
 };
 
 #endif //CONTINUOUS_CONVEX_COLLISION_DEMO_H

Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.h

@@ -28,10 +28,7 @@ class ConvexDecompositionDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
+	
-	{
-		//not yet
-	}
 	
 };
 

Demos/GjkConvexCastDemo/LinearConvexCastDemo.h

@@ -28,9 +28,7 @@ class LinearConvexCastDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
+	
-	{
-	}
 };
 
 #endif //LINEAR_CONVEX_CAST_DEMO_H

Demos/OpenGL/DemoApplication.cpp

@@ -21,8 +21,12 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"//picking
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+
 #include "GL_ShapeDrawer.h"
 #include "LinearMath/btQuickprof.h"
+#include "LinearMath/btDefaultMotionState.h"
+
 #include "BMF_Api.h"
 
 extern bool gDisableDeactivation;
@@ -646,7 +650,9 @@ btRigidBody*	DemoApplication::localCreateRigidBody(float mass, const btTransform
 	if (isDynamic)
 		shape->calculateLocalInertia(mass,localInertia);
 
-	btRigidBody* body = new btRigidBody(mass,startTransform,shape,localInertia);
+	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+	btRigidBody* body = new btRigidBody(mass,myMotionState,shape,localInertia);
+	body->m_userObjectPointer = myMotionState;
 
 	m_dynamicsWorld->addRigidBody(body);
 	
@@ -656,7 +662,6 @@ btRigidBody*	DemoApplication::localCreateRigidBody(float mass, const btTransform
 
 
 
-
 void DemoApplication::renderme()
 {
 	updateCamera();
@@ -670,7 +675,15 @@ void DemoApplication::renderme()
 		for (int i=0;i<numObjects;i++)
 		{
 			btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
-			colObj->m_worldTransform.getOpenGLMatrix(m);
+			
+			if (colObj->m_userObjectPointer)
+			{
+				btDefaultMotionState* myMotionState = (btDefaultMotionState*)colObj->m_userObjectPointer;
+				myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
+			} else
+			{
+				colObj->m_worldTransform.getOpenGLMatrix(m);
+			}
 
 			btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
 			if (i & 1)
@@ -798,3 +811,27 @@ void DemoApplication::renderme()
 	}
 
 }
+
+void	DemoApplication::clientResetScene()
+{
+	int numObjects = m_dynamicsWorld->getNumCollisionObjects();
+	
+	for (int i=0;i<numObjects;i++)
+	{
+		btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		
+		if (colObj->m_userObjectPointer)
+		{
+			btDefaultMotionState* myMotionState = (btDefaultMotionState*)colObj->m_userObjectPointer;
+			myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
+			colObj->m_worldTransform = myMotionState->m_graphicsWorldTrans;
+			colObj->m_interpolationWorldTransform = myMotionState->m_startWorldTrans;
+			btRigidBody* body = btRigidBody::upcast(colObj);
+			if (body && !body->isStaticObject())
+			{
+				btRigidBody::upcast(colObj)->setLinearVelocity(btVector3(0,0,0));
+				btRigidBody::upcast(colObj)->setAngularVelocity(btVector3(0,0,0));
+			}
+		}
+	}
+}

Demos/OpenGL/DemoApplication.h

@@ -38,6 +38,7 @@ subject to the following restrictions:
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btTransform.h"
+#include "LinearMath/btQuickProf.h"
 
 class	btCollisionShape;
 class	btDynamicsWorld;
@@ -50,6 +51,8 @@ class DemoApplication
 	protected:
 		
 
+	hidden::Clock m_clock;
+
 	///this is the most important class
 	btDynamicsWorld*		m_dynamicsWorld;
 
@@ -134,7 +137,7 @@ public:
 
 	virtual void clientMoveAndDisplay() = 0;
 
-	virtual void	clientResetScene() =0 ;
+	virtual void	clientResetScene();
 
 	///Demo functions
 	void	shootBox(const btVector3& destination);

Demos/Raytracer/Raytracer.h

@@ -28,9 +28,7 @@ class Raytracer : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
+	
-	{
-	}
 };
 
 #endif //RAYTRACER_H

Demos/SimplexDemo/SimplexDemo.h

@@ -28,9 +28,7 @@ class SimplexDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene()
+	
-	{
-	}
 };
 
 #endif //SIMPLEX_DEMO_H

Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp

@@ -154,23 +154,7 @@ void UserCollisionAlgorithm::clientMoveAndDisplay()
 
 }
 
-void	UserCollisionAlgorithm::clientResetScene()
-{
-	/*
-	int numObj = m_physicsEnvironmentPtr->GetNumControllers();
 
-	//skip ground
-	for (int i=1;i<numObj;i++)
-	{
-		CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i);
-		ctrl->setPosition(1,2*i,1);
-		ctrl->setOrientation(0,0,0,1);
-		ctrl->SetLinearVelocity(0,0,0,0);
-		ctrl->SetAngularVelocity(0,0,0,0);
-	}
-	*/
-
-}
 
 
 

Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.h

@@ -28,7 +28,7 @@ class UserCollisionAlgorithm : public DemoApplication
 
 	virtual void displayCallback();
 	
-	virtual void	clientResetScene();
+	
 	
 };
 

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -21,8 +21,9 @@ btCollisionObject::btCollisionObject()
 		m_collisionFlags(0),
 		m_activationState1(1),
 		m_deactivationTime(0.f),
+		m_userObjectPointer(0),
 		m_hitFraction(1.f),
-		m_ccdSweptShereRadius(0.f),
+		m_ccdSweptSphereRadius(0.f),
 		m_ccdSquareMotionTreshold(0.f)
 {
 	

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -27,6 +27,9 @@ subject to the following restrictions:
 
 struct	btBroadphaseProxy;
 class	btCollisionShape;
+#include "LinearMath/btMotionState.h"
+
+
 
 /// btCollisionObject can be used to manage collision detection objects. 
 /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
@@ -68,7 +71,7 @@ struct	btCollisionObject
 	float			m_hitFraction; 
 	
 	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
-	float			m_ccdSweptShereRadius;
+	float			m_ccdSweptSphereRadius;
 
 	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold
 	float			m_ccdSquareMotionTreshold;

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -271,12 +271,12 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
 		rayAabbMin.setMin(convexToLocal.getOrigin());
 		btVector3 rayAabbMax = convexFromLocal.getOrigin();
 		rayAabbMax.setMax(convexToLocal.getOrigin());
-		rayAabbMin -= btVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius);
+		rayAabbMin -= btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius);
-		rayAabbMax += btVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius);
+		rayAabbMax += btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius);
 
 		float curHitFraction = 1.f; //is this available?
 		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
-		convexbody->m_ccdSweptShereRadius,curHitFraction);
+		convexbody->m_ccdSweptSphereRadius,curHitFraction);
 
 		raycastCallback.m_hitFraction = convexbody->m_hitFraction;
 

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -214,7 +214,7 @@ float	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
 	{
 		btConvexShape* convex0 = static_cast<btConvexShape*>(col0->m_collisionShape);
 
-		btSphereShape	sphere1(col1->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation
+		btSphereShape	sphere1(col1->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
 		btConvexCast::CastResult result;
 		btVoronoiSimplexSolver voronoiSimplex;
 		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
@@ -247,7 +247,7 @@ float	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
 	{
 		btConvexShape* convex1 = static_cast<btConvexShape*>(col1->m_collisionShape);
 
-		btSphereShape	sphere0(col0->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation
+		btSphereShape	sphere0(col0->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
 		btConvexCast::CastResult result;
 		btVoronoiSimplexSolver voronoiSimplex;
 		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -22,6 +22,7 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include <LinearMath/btTransformUtil.h>
 
 //rigidbody & constraints
 #include "BulletDynamics/Dynamics/btRigidBody.h"
@@ -29,6 +30,20 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 
+//for debug rendering
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+
 //vehicle
 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
@@ -46,6 +61,7 @@ btDiscreteDynamicsWorld::btDiscreteDynamicsWorld()
 m_constraintSolver(new btSequentialImpulseConstraintSolver),
 m_debugDrawer(0),
 m_gravity(0,-10,0),
+m_localTime(1.f/60.f),
 m_profileTimings(0)
 {
 	m_islandManager = new btSimulationIslandManager();
@@ -59,6 +75,7 @@ btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOver
 m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
 m_debugDrawer(0),
 m_gravity(0,-10,0),
+m_localTime(1.f/60.f),
 m_profileTimings(0)
 {
 	m_islandManager = new btSimulationIslandManager();
@@ -104,13 +121,37 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 	for (unsigned int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
+		if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+		{
+			btVector3 color(255.f,255.f,255.f);
+			switch(colObj->GetActivationState())
+			{
+			case  ACTIVE_TAG:
+				color = btVector3(255.f,255.f,255.f);
+			case ISLAND_SLEEPING:
+				color =  btVector3(0.f,255.f,0.f);
+			case WANTS_DEACTIVATION:
+				color = btVector3(0.f,255.f,255.f);
+			case DISABLE_DEACTIVATION:
+				color = btVector3(255.f,0.f,0.f);
+			case DISABLE_SIMULATION:
+				color = btVector3(255.f,255.f,0.f);
+			default:
+				{
+					color = btVector3(255.f,0.f,0.f);
+				}
+			};
+
+			debugDrawObject(colObj->m_worldTransform,colObj->m_collisionShape,color);
+		}
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body && body->getMotionState())
 		{
 			if (body->GetActivationState() != ISLAND_SLEEPING)
 			{
-				body->getMotionState()->setWorldOrientation(body->getCenterOfMassTransform().getRotation());
+				btTransform interpolatedTransform;
-				body->getMotionState()->setWorldPosition(body->getCenterOfMassTransform().getOrigin());
+				btTransformUtil::integrateTransform(body->m_interpolationWorldTransform,body->getLinearVelocity(),body->getAngularVelocity(),m_localTime,interpolatedTransform);
+				body->getMotionState()->setWorldTransform(interpolatedTransform);
 			}
 		}
 	}
@@ -118,29 +159,51 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 }
 
 
-void	btDiscreteDynamicsWorld::stepSimulation(float timeStep, int numSubsteps)
+int	btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep)
 {
+	int numSimulationSubSteps = 0;
+
+	if (maxSubSteps)
+	{
+		//fixed timestep with interpolation
+		m_localTime += timeStep;
+		if (m_localTime >= fixedTimeStep)
+		{
+			numSimulationSubSteps = int( m_localTime / fixedTimeStep);
+			m_localTime -= numSimulationSubSteps * fixedTimeStep;
+		}
+	} else
+	{
+		//variable timestep
+		fixedTimeStep = timeStep;
+		m_localTime = timeStep;
+		numSimulationSubSteps = 1;
+		maxSubSteps = 1;
+	}
 
 	//process some debugging flags
 	if (getDebugDrawer())
 	{
 		gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
 	}
-	if (!btFuzzyZero(timeStep) && numSubsteps)
+	if (!btFuzzyZero(timeStep) && numSimulationSubSteps)
 	{
 
-		saveKinematicState(timeStep);
+		saveKinematicState(fixedTimeStep);
 
-		int i;
+		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
-		float subTimeStep = timeStep / float(numSubsteps);
+		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
 
-		for (i=0;i<numSubsteps;i++)
+		for (int i=0;i<clampedSimulationSteps;i++)
 		{
-			internalSingleStepSimulation(subTimeStep);
+			internalSingleStepSimulation(fixedTimeStep);
 		}
 
-		synchronizeMotionStates();
 	} 
+
+	synchronizeMotionStates();
+
+	return numSimulationSubSteps;
 }
 
 void	btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
@@ -206,12 +269,8 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 {
 	body->setGravity(m_gravity);
 	bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
-	short collisionFilterGroup = isDynamic? 
+	short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter;
-	btBroadphaseProxy::DefaultFilter : 
+	short collisionFilterMask = isDynamic? 	btBroadphaseProxy::AllFilter : 	btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
-	btBroadphaseProxy::StaticFilter;
-	short collisionFilterMask = isDynamic? 
-	btBroadphaseProxy::AllFilter : 
-	btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
 
 	addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
 }
@@ -537,3 +596,151 @@ void	btDiscreteDynamicsWorld::startProfiling(float timeStep)
 }
 
 
+
+
+	
+
+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_worldTrans(worldTrans),
+		m_color(color)
+	{
+	}
+
+	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+	{
+		processTriangle(triangle,partId,triangleIndex);
+	}
+
+	virtual void processTriangle(btVector3* triangle,int partId, int 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::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
+
+	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);
+				float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
+				btVector3 start = worldTransform.getOrigin();
+				getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(radius,0,0),color);
+				getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,radius,0),color);
+				getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,0,radius),color);
+				//drawSphere					
+				break;
+			}
+		case MULTI_SPHERE_SHAPE_PROXYTYPE:
+		case CONE_SHAPE_PROXYTYPE:
+			{
+				const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+				float radius = coneShape->getRadius();//+coneShape->getMargin();
+				float height = coneShape->getHeight();//+coneShape->getMargin();
+				btVector3 start = worldTransform.getOrigin();
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(radius,0,-0.5*height),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(-radius,0,-0.5*height),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(0,radius,-0.5*height),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(0,-radius,-0.5*height),color);
+				break;
+
+			}
+		case CYLINDER_SHAPE_PROXYTYPE:
+			{
+				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
+				int upAxis = cylinder->getUpAxis();
+				float radius = cylinder->getRadius();
+				float halfHeight = cylinder->getHalfExtents()[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;
+			}
+		default:
+			{
+
+				if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+				{
+					btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape;
+					//btVector3 aabbMax(1e30f,1e30f,1e30f);
+					//btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
+
+					//todo pass camera, for some culling
+					btVector3 aabbMax(1e30f,1e30f,1e30f);
+					btVector3 aabbMin(-1e30f,-1e30f,-1e30f);
+
+					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(1e30f,1e30f,1e30f);
+					btVector3 aabbMin(-1e30f,-1e30f,-1e30f);
+					//DebugDrawcallback drawCallback;
+					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+					convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+				}
+
+
+				/// for polyhedral shapes
+				if (shape->isPolyhedral())
+				{
+					btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
+
+					int i;
+					for (i=0;i<polyshape->getNumEdges();i++)
+					{
+						btPoint3 a,b;
+						polyshape->getEdge(i,a,b);
+						btVector3 wa = worldTransform * a;
+						btVector3 wb = worldTransform * b;
+						getDebugDrawer()->drawLine(wa,wb,color);
+
+					}
+
+					
+				}
+			}
+		}
+	}
+}

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -45,6 +45,10 @@ protected:
 
 	btVector3	m_gravity;
 
+	//for variable timesteps
+	float	m_localTime;
+	//for variable timesteps
+
 	bool	m_ownsIslandManager;
 	bool	m_ownsConstraintSolver;
 
@@ -74,6 +78,7 @@ protected:
 
 	void	saveKinematicState(float timeStep);
 
+
 public:
 
 
@@ -85,7 +90,8 @@ public:
 		
 	virtual ~btDiscreteDynamicsWorld();
 
-	virtual void	stepSimulation( float timeStep, int numSubsteps=1);
+	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
+	virtual int	stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f);
 
 	virtual void	updateAabbs();
 
@@ -128,6 +134,9 @@ public:
 
 	virtual void	removeRigidBody(btRigidBody* body);
 
+	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+
+
 };
 
 #endif //BT_DISCRETE_DYNAMICS_WORLD_H

src/BulletDynamics/Dynamics/btDynamicsWorld.h

@@ -38,7 +38,8 @@ class btDynamicsWorld : public btCollisionWorld
 		}
 		
 		///stepSimulation proceeds the simulation over timeStep units
-		virtual void	stepSimulation( float timeStep,int numSubsteps=1) = 0;
+		///if maxSubSteps > 0, it will interpolate time steps
+		virtual int		stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f)=0;
 			
 		virtual void	updateAabbs() = 0;
 				

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -43,11 +43,9 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap
 	m_frictionSolverType(0)
 {
 
-	btQuaternion worldOrn;
+	motionState->getWorldTransform(m_worldTransform);
-	btVector3	worldPos;
+
-	motionState->getWorldOrientation(worldOrn);
+	m_interpolationWorldTransform = m_worldTransform;
-	motionState->getWorldPosition(worldPos);
-	m_worldTransform = btTransform(worldOrn,worldPos);
 
 	//moved to btCollisionObject
 	m_friction = friction;
@@ -82,6 +80,7 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 {
 	
 	m_worldTransform = worldTransform;
+	m_interpolationWorldTransform = m_worldTransform;
 
 	//moved to btCollisionObject
 	m_friction = friction;
@@ -118,9 +117,6 @@ void			btRigidBody::saveKinematicState(btScalar timeStep)
 		//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
 	}
 	
-
-	m_interpolationWorldTransform = m_worldTransform;
-	
 	m_kinematicTimeStep = timeStep;
 }
 	
@@ -260,6 +256,7 @@ btQuaternion btRigidBody::getOrientation() const
 	
 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 {
+	m_interpolationWorldTransform = m_worldTransform;
 	m_worldTransform = xform;
 	updateInertiaTensor();
 }

src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

@@ -47,7 +47,7 @@ btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
 		delete m_constraintSolver;
 }
 
-void	btSimpleDynamicsWorld::stepSimulation(float timeStep,int numSubsteps)
+int		btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep)
 {
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
@@ -72,6 +72,10 @@ void	btSimpleDynamicsWorld::stepSimulation(float timeStep,int numSubsteps)
 		
 	updateAabbs();
 
+	synchronizeMotionStates();
+
+	return 1;
+
 }
 
 
@@ -161,3 +165,22 @@ void	btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep)
 		}
 	}
 }
+
+
+void	btSimpleDynamicsWorld::synchronizeMotionStates()
+{
+	//todo: iterate over awake simulation islands!
+	for (unsigned 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->m_worldTransform);
+			}
+		}
+	}
+
+}

src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

@@ -54,7 +54,8 @@ public:
 
 	virtual ~btSimpleDynamicsWorld();
 		
-	virtual void	stepSimulation( float timeStep,int numSubsteps=1);
+	///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
+	virtual int	stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f);
 
 	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer) 
 	{
@@ -73,6 +74,9 @@ public:
 	virtual void	removeRigidBody(btRigidBody* body);
 	
 	virtual void	updateAabbs();
+
+	void	synchronizeMotionStates();
+
 };
 
 #endif //BT_SIMPLE_DYNAMICS_WORLD_H

src/LinearMath/btDefaultMotionState.h

@@ -0,0 +1,32 @@
+#ifndef DEFAULT_MOTION_STATE_H
+#define DEFAULT_MOTION_STATE_H
+
+///btDefaultMotionState provides a common implementation to synchronize world transforms with offsets
+struct	btDefaultMotionState : public btMotionState
+{
+	btTransform m_graphicsWorldTrans;
+	btTransform	m_centerOfMassOffset;
+	btTransform m_startWorldTrans;
+
+	btDefaultMotionState(const btTransform& startTrans,const btTransform& centerOfMassOffset = btTransform::getIdentity())
+		: m_graphicsWorldTrans(startTrans),
+		m_centerOfMassOffset(centerOfMassOffset),
+		m_startWorldTrans(startTrans)
+
+	{
+	}
+
+	///synchronizes world transform from user to physics
+	virtual void	getWorldTransform(btTransform& centerOfMassWorldTrans )
+	{
+			centerOfMassWorldTrans = 	m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ;
+	}
+
+	///synchronizes world transform from physics to user
+	virtual void	setWorldTransform(const btTransform& centerOfMassWorldTrans)
+	{
+			m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
+	}
+};
+
+#endif //DEFAULT_MOTION_STATE_H

src/LinearMath/btIDebugDraw.h

@@ -38,13 +38,13 @@ class	btIDebugDraw
 	enum	DebugDrawModes
 	{
 		DBG_NoDebug=0,
-		DBG_DrawAabb=1,
+		DBG_DrawWireframe = 1,
-		DBG_DrawText=2,
+		DBG_DrawAabb=2,
 		DBG_DrawFeaturesText=4,
 		DBG_DrawContactPoints=8,
 		DBG_NoDeactivation=16,
 		DBG_NoHelpText = 32,
-		DBG_DrawWireframe = 64,
+		DBG_DrawText=64,
 		DBG_ProfileTimings = 128,
 		DBG_EnableSatComparison = 256,
 		DBG_DisableBulletLCP = 512,

src/LinearMath/btMotionState.h

@@ -16,9 +16,7 @@ subject to the following restrictions:
 #ifndef BT_MOTIONSTATE_H
 #define BT_MOTIONSTATE_H
 
-#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
-#include "LinearMath/btQuaternion.h"
-
 
 ///btMotionState allows the dynamics world to synchronize the updated world transforms with graphics
 ///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation)
@@ -31,14 +29,9 @@ class	btMotionState
 			
 		}
 		
-		virtual void	getWorldPosition(btVector3& worldPos )=0;
+		virtual void	getWorldTransform(btTransform& worldTrans )=0;
-		virtual void	getWorldScaling(btVector3& scaling)=0;
-		virtual void	getWorldOrientation(btQuaternion& orn)=0;
 
-		virtual void	setWorldPosition(const btVector3& worldPos)=0;
+		virtual void	setWorldTransform(const btTransform& worldTrans)=0;
-		virtual	void	setWorldOrientation(const btQuaternion& orn)=0;
-
-		virtual	void	calculateWorldTransformations()=0;
 
 };