add "latency motion state interpolation", using previous frame/current, instead of current->next

See Demos/ForkLiftDemo for example (F7 to toggle, use single-frame stepping, press 'i' and 's' to see the effect)

Demos/ForkLiftDemo/ForkLiftDemo.cpp

@@ -465,7 +465,7 @@ void ForkLiftDemo::renderme()
 	}
 
 
-	int lineWidth=250;
+	int lineWidth=400;
 	int xStart = m_glutScreenWidth - lineWidth;
 	int yStart = 20;
 
@@ -476,39 +476,44 @@ void ForkLiftDemo::renderme()
 		glColor3f(0, 0, 0);
 		char buf[124];
 		
-		glRasterPos3f(xStart, yStart, 0);
 		sprintf(buf,"SHIFT+Cursor Left/Right - rotate lift");
 		GLDebugDrawString(xStart,20,buf);
 		yStart+=20;
-		glRasterPos3f(xStart, yStart, 0);
+		sprintf(buf,"SHIFT+Cursor UP/Down - fork up/down");
-		sprintf(buf,"SHIFT+Cursor UP/Down - move fork up/down");
 		yStart+=20;
 		GLDebugDrawString(xStart,yStart,buf);
 
-		yStart+=20;
+		if (m_useDefaultCamera)
-		glRasterPos3f(xStart, yStart, 0);
-		sprintf(buf,"F6 - toggle solver");
-		GLDebugDrawString(xStart,yStart,buf);
-
-		yStart+=20;
-		glRasterPos3f(xStart, yStart, 0);
-		if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER)
 		{
-			sprintf(buf,"Using direct MLCP solver");
+			sprintf(buf,"F5 - camera mode (free)");
 		} else
 		{
-			sprintf(buf,"Using sequential impulse solver");
+			sprintf(buf,"F5 - camera mode (follow)");
 		}
-		GLDebugDrawString(xStart,yStart,buf);
-
-		
-
-		glRasterPos3f(xStart, yStart, 0);
-		sprintf(buf,"F5 - toggle camera mode");
 		yStart+=20;
 		GLDebugDrawString(xStart,yStart,buf);
-		glRasterPos3f(xStart, yStart, 0);
+
-        sprintf(buf,"Click inside this window for keyboard focus");
+		yStart+=20;
+		if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER)
+		{
+			sprintf(buf,"F6 - solver (direct MLCP)");
+		} else
+		{
+			sprintf(buf,"F6 - solver (sequential impulse)");
+		}
+		GLDebugDrawString(xStart,yStart,buf);
+		btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) m_dynamicsWorld;
+		if (world->getLatencyMotionStateInterpolation())
+		{
+			sprintf(buf,"F7 - motionstate interpolation (on)");
+		} else
+		{
+			sprintf(buf,"F7 - motionstate interpolation (off)");
+		}
+		yStart+=20;
+		GLDebugDrawString(xStart,yStart,buf);
+
+		sprintf(buf,"Click window for keyboard focus");
 		yStart+=20;
 		GLDebugDrawString(xStart,yStart,buf);
 
@@ -809,6 +814,13 @@ void ForkLiftDemo::specialKeyboard(int key, int x, int y)
 					break;
 				}
 
+			case GLUT_KEY_F7:
+				{
+					btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*)m_dynamicsWorld;
+					world->setLatencyMotionStateInterpolation(!world->getLatencyMotionStateInterpolation());
+					printf("world latencyMotionStateInterpolation = %d\n", world->getLatencyMotionStateInterpolation());
+					break;
+				}
 			case GLUT_KEY_F6:
 				{
 					//switch solver (needs demo restart)

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -208,7 +208,9 @@ m_gravity(0,-10,0),
 m_localTime(0),
 m_synchronizeAllMotionStates(false),
 m_applySpeculativeContactRestitution(false),
-m_profileTimings(0)
+m_profileTimings(0),
+m_fixedTimeStep(0),
+m_latencyMotionStateInterpolation(true)
 
 {
 	if (!m_constraintSolver)
@@ -357,7 +359,9 @@ void	btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
 		{
 			btTransform interpolatedTransform;
 			btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-				body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
+				body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
+				(m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
+				interpolatedTransform);
 			body->getMotionState()->setWorldTransform(interpolatedTransform);
 		}
 	}
@@ -401,6 +405,7 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 	if (maxSubSteps)
 	{
 		//fixed timestep with interpolation
+		m_fixedTimeStep = fixedTimeStep;
 		m_localTime += timeStep;
 		if (m_localTime >= fixedTimeStep)
 		{
@@ -411,7 +416,8 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 	{
 		//variable timestep
 		fixedTimeStep = timeStep;
-		m_localTime = timeStep;
+		m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
+		m_fixedTimeStep = 0;
 		if (btFuzzyZero(timeStep))
 		{
 			numSimulationSubSteps = 0;

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -53,6 +53,7 @@ protected:
 
 	//for variable timesteps
 	btScalar	m_localTime;
+	btScalar	m_fixedTimeStep;
 	//for variable timesteps
 
 	bool	m_ownsIslandManager;
@@ -64,6 +65,8 @@ protected:
 	
 	int	m_profileTimings;
 
+	bool	m_latencyMotionStateInterpolation;
+
 	btAlignedObjectArray<btPersistentManifold*>	m_predictiveManifolds;
 
 	virtual void	predictUnconstraintMotion(btScalar timeStep);
@@ -216,6 +219,16 @@ public:
 	///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