Update the physics engine: interpolate between frames so track to/camera actuators work properly. (bug #1816)

6 files changed, 382 insertions, 139 deletions

diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h
index b0e0717cf8c..08e29265d0c 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h
+++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h
@@ -61,6 +61,7 @@ public:
 	void integrateBackward(MT_Scalar timeStep, const MT_Vector3 &velocity, const MT_Quaternion& ang_vel);
 	void integrateForward(MT_Scalar timeStep, const SM_MotionState &prev_state);
 	
+	void lerp(const SM_MotionState &prev, const SM_MotionState &next);
 	void lerp(MT_Scalar t, const SM_MotionState &other);
 	
 	virtual MT_Transform getTransform() const {
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
index b37e1a5466b..07eb5a7637f 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
+++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
@@ -81,7 +81,8 @@ public:
  * It encapsulates an object in the physics scene, and is responsible
  * for calculating the collision response of objects.
  */
-class SM_Object : public SM_MotionState {
+class SM_Object 
+{
 public:
 	SM_Object() ;
 	SM_Object(
@@ -266,7 +267,29 @@ public:
 	
 	void relax();
 	
+	SM_MotionState &getCurrentFrame();
+	SM_MotionState &getPreviousFrame();
+	SM_MotionState &getNextFrame();
+	
+	const SM_MotionState &getCurrentFrame()   const;
+	const SM_MotionState &getPreviousFrame()  const;
+	const SM_MotionState &getNextFrame()      const;
+
+	// Motion state functions	
+	const MT_Point3&     getPosition()        const;
+	const MT_Quaternion& getOrientation()     const;
+	const MT_Vector3&    getLinearVelocity()  const;
+	const MT_Vector3&    getAngularVelocity() const;
+	
+	MT_Scalar            getTime()            const;
+	
+	void setTime(MT_Scalar time);
+	
+	void interpolate(MT_Scalar timeStep);
+	void endFrame();
+	
 private:
+	friend class Contact;
 	// Tweak parameters
 	static MT_Scalar ImpulseThreshold;
 
@@ -324,14 +347,12 @@ private:
 	MT_Vector3              m_reaction_impulse;      // The accumulated impulse resulting from collisions
 	MT_Vector3              m_reaction_force;        // The reaction force derived from the reaction impulse   
 
-	unsigned int            m_kinematic      : 1;    // Have I been displaced (translated, rotated, scaled) in this frame? 
-	unsigned int            m_prev_kinematic : 1;    // Have I been displaced (translated, rotated, scaled) in the previous frame? 
-	unsigned int            m_is_rigid_body  : 1;    // Should friction give me a change in angular momentum?
-
 	MT_Vector3              m_lin_mom;               // Linear momentum (linear velocity times mass)
 	MT_Vector3              m_ang_mom;               // Angular momentum (angualr velocity times inertia)
 	MT_Vector3              m_force;                 // Force on center of mass (afffects linear momentum)
-	MT_Vector3              m_torque;                // Torque around center of mass (affects angualr momentum)
+	MT_Vector3              m_torque;                // Torque around center of mass (affects angular momentum)
+	
+	SM_MotionState          m_frames[3];             
 	
 	MT_Vector3              m_error;                 // Error in position:- amount object must be moved to prevent intersection with scene
 
@@ -355,6 +376,12 @@ private:
 	MT_Scalar               m_inv_mass;              // 1/mass
 	MT_Vector3              m_inv_inertia;           // [1/inertia_x, 1/inertia_y, 1/inertia_z]
 	MT_Matrix3x3            m_inv_inertia_tensor;    // Inverse Inertia Tensor
+	
+	bool                    m_kinematic;             // Have I been displaced (translated, rotated, scaled) in this frame? 
+	bool                    m_prev_kinematic;        // Have I been displaced (translated, rotated, scaled) in the previous frame? 
+	bool                    m_is_rigid_body;         // Should friction give me a change in angular momentum?
+	int                     m_static;                // temporarily static.
+
 };
 
 #endif
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h
index 80e8c635cbb..a0d5c983031 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h
+++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h
@@ -104,6 +104,7 @@ public:
 	// 'subSampling' can be used to control aliasing effects
 	// (fast moving objects traversing through walls and such). 
 	bool proceed(MT_Scalar curtime, MT_Scalar ticrate);
+	void proceed(MT_Scalar subStep);
 
 	/**
 	 * Test whether any objects lie on the line defined by from and
@@ -167,7 +168,7 @@ private:
 	 * collision tests. */
 	T_ObjectList        m_objectList;
 	
-	MT_Scalar           m_lastTime;
+	unsigned int        m_frames;
 };
 
 #endif
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp
index a3285b19116..1fadbdadfa0 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp
+++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp
@@ -29,7 +29,7 @@
  *
  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  */
-
+#include <MT_Scalar.h>
 #include <MT_Vector3.h>
 #include <MT_Quaternion.h>
  
@@ -56,15 +56,47 @@ void SM_MotionState::integrateForward(MT_Scalar timeStep, const SM_MotionState &
 	m_orn.normalize();
 }
 
+/*
+// Newtonian lerp: interpolate based on Newtonian motion
+void SM_MotionState::nlerp(const SM_MotionState &prev, const SM_MotionState &next)
+{
+	MT_Scalar dt = next.getTime() - prev.getTime();
+	MT_Scalar t = getTime() - prev.getTime();
+	MT_Vector3 dx = next.getPosition() - prev.getPosition();
+	MT_Vector3 a = dx/(dt*dt) - prev.getLinearVelocity()/dt;
+	
+	m_pos = prev.getPosition() + prev.getLinearVelocity()*t + a*t*t;
+}
+*/
+
+void SM_MotionState::lerp(const SM_MotionState &prev, const SM_MotionState &next)
+{
+	MT_Scalar dt = next.getTime() - prev.getTime();
+	if (MT_fuzzyZero(dt))
+	{
+		*this = next;
+		return;
+	}
+	
+	MT_Scalar x = (getTime() - prev.getTime())/dt;
+	
+	m_pos = x*next.getPosition() + (1-x)*prev.getPosition();
+	
+	m_orn = prev.getOrientation().slerp(next.getOrientation(), 1-x);
+	
+	m_lin_vel = x*next.getLinearVelocity() + (1-x)*prev.getLinearVelocity();
+	m_ang_vel = x*next.getAngularVelocity() + (1-x)*prev.getAngularVelocity();
+}
+
 void SM_MotionState::lerp(MT_Scalar t, const SM_MotionState &other)
 {
 	MT_Scalar x = (t - getTime())/(other.getTime() - getTime());
-	m_pos = x*m_pos + (1-x)*other.getPosition();
+	m_pos = (1-x)*m_pos + x*other.getPosition();
 	
-	m_orn = m_orn.slerp(other.getOrientation(), x);
+	m_orn =  other.getOrientation().slerp(m_orn, x);
 	
-	m_lin_vel = x*m_lin_vel + (1-x)*other.getLinearVelocity();
-	m_ang_vel = x*m_ang_vel + (1-x)*other.getAngularVelocity();
+	m_lin_vel = (1-x)*m_lin_vel + x*other.getLinearVelocity();
+	m_ang_vel = (1-x)*m_ang_vel + x*other.getAngularVelocity();
 	
 	m_time = t;
 }
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
index ed5171a87a2..cab191707be 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
+++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
@@ -49,7 +49,95 @@
 
 #include "MT_MinMax.h"
 
-MT_Scalar SM_Object::ImpulseThreshold = -10.;
+MT_Scalar SM_Object::ImpulseThreshold = -0.01;
+
+struct Contact
+{
+	SM_Object *obj1;
+	SM_Object *obj2;
+	MT_Vector3 normal;
+	MT_Point3 pos;
+	
+	// Sort objects by height
+	bool operator()(const Contact *a, const Contact *b)
+	{
+		return a->pos[2] < b->pos[2];
+	}
+	
+	Contact(SM_Object *o1, SM_Object *o2, const MT_Vector3 nor, const MT_Point3 p)
+		: obj1(o1),
+		  obj2(o2),
+		  normal(nor),
+		  pos(p)
+	{
+	}
+	
+	Contact()
+	{
+	}
+	
+	void resolve()
+	{
+		if (obj1->m_static || obj2->m_static)
+		{
+			if (obj1->isDynamic())
+			{
+				if (obj1->m_static && obj2->m_static)
+				{
+					if (obj1->m_static < obj2->m_static)
+					{
+						obj2->m_error -= normal;
+						obj2->m_static = obj1->m_static + 1;
+					}
+					else
+					{
+						obj1->m_error += normal;
+						obj1->m_static = obj2->m_static + 1;
+					}
+				}
+				else
+				{
+					if (obj1->m_static)
+					{
+						obj2->m_error -= normal;
+						obj2->m_static = obj1->m_static + 1;
+					}
+					else
+					{
+						obj1->m_error += normal;
+						obj1->m_static = obj2->m_static + 1;
+					}
+				}
+			}
+			else
+			{
+				obj2->m_error -= normal;
+				obj2->m_static = 1;
+			}
+		}
+		else
+		{
+			// This distinction between dynamic and non-dynamic objects should not be 
+			// necessary. Non-dynamic objects are assumed to have infinite mass.
+			if (obj1->isDynamic()) {
+				MT_Vector3 error = normal * 0.5f;
+				obj1->m_error += error;
+				obj2->m_error -= error;
+			}
+			else {
+				// Same again but now obj1 is non-dynamic
+				obj2->m_error -= normal;
+				obj2->m_static = obj1->m_static + 1;
+			}
+		}
+	
+	}
+		
+	
+	typedef std::set<Contact*, Contact> Set;
+};
+
+static Contact::Set contacts;
 
 SM_Object::SM_Object(
 	DT_ShapeHandle shape, 
@@ -69,9 +157,6 @@ SM_Object::SM_Object(
 	m_scaling(1.0, 1.0, 1.0),
 	m_reaction_impulse(0.0, 0.0, 0.0),
 	m_reaction_force(0.0, 0.0, 0.0),
-	m_kinematic(false),
-	m_prev_kinematic(false),
-	m_is_rigid_body(false),
 	m_lin_mom(0.0, 0.0, 0.0),
 	m_ang_mom(0.0, 0.0, 0.0),
 	m_force(0.0, 0.0, 0.0),
@@ -81,7 +166,11 @@ SM_Object::SM_Object(
 	m_combined_ang_vel (0.0, 0.0, 0.0),
 	m_fh_object(0),
 	m_inv_mass(0.0),
-	m_inv_inertia(0., 0., 0.)
+	m_inv_inertia(0., 0., 0.),
+	m_kinematic(false),
+	m_prev_kinematic(false),
+	m_is_rigid_body(false),
+	m_static(0)
 {
 	m_object = DT_CreateObject(this, shape);
 	m_xform.setIdentity();
@@ -108,7 +197,7 @@ integrateForces(
 	MT_Scalar timeStep
 ){
 	if (!m_suspended) {
-		m_prev_state = *this;
+		m_prev_state = getNextFrame();
 		m_prev_state.setLinearVelocity(actualLinVelocity());
 		m_prev_state.setAngularVelocity(actualAngVelocity());
 		if (isDynamic()) {
@@ -119,8 +208,8 @@ integrateForces(
 			m_lin_mom *= pow(m_shapeProps->m_lin_drag, timeStep);
 			m_ang_mom *= pow(m_shapeProps->m_ang_drag, timeStep);
 			// Set velocities according momentum
-			m_lin_vel = m_lin_mom * m_inv_mass;
-			m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
+			getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass);
+			getNextFrame().setAngularVelocity(m_inv_inertia_tensor * m_ang_mom);
 		}
 	}	
 
@@ -146,13 +235,13 @@ integrateMomentum(
 //#define BACKWARD
 #ifdef  MIDPOINT
 // Midpoint rule
-		integrateMidpoint(timeStep, m_prev_state, actualLinVelocity(), actualAngVelocity());
+		getNextFrame().integrateMidpoint(timeStep, m_prev_state, actualLinVelocity(), actualAngVelocity());
 #elif defined BACKWARD
 // Backward Euler
-		integrateBackward(timeStep, actualLinVelocity(), actualAngVelocity());
+		getNextFrame().integrateBackward(timeStep, actualLinVelocity(), actualAngVelocity());
 #else 
 // Forward Euler
-		integrateForward(timeStep, m_prev_state);
+		getNextFrame().integrateForward(timeStep, m_prev_state);
 #endif
 
 		calcXform();
@@ -194,7 +283,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
 		 * if rel_vel_normal < ImpulseThreshold, scale the restitution down.
 		 * This should improve the simulation where the object is stacked.
 		 */
-		restitution *= MT_min(MT_Scalar(1.0), rel_vel_normal/ImpulseThreshold);
+		restitution *= MT_min(MT_Scalar(1.0), m_shapeProps->m_mass*rel_vel_normal/ImpulseThreshold);
 				
 		MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal;
 		
@@ -207,7 +296,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
 			 * Apply impulse at the collision point.
 			 * Take rotational inertia into account.
 			 */
-			applyImpulse(local2 + getPosition(), impulse * normal);
+			applyImpulse(local2 + getNextFrame().getPosition(), impulse * normal);
 		} else {
 			/**
 			 * Apply impulse through object centre. (no rotation.)
@@ -244,7 +333,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
 			 * local coordinates.
 			 */
 
-			MT_Matrix3x3 lcs(m_orn);
+			MT_Matrix3x3 lcs(getNextFrame().getOrientation());
 			
 			/**
 			 * We cannot use m_xform.getBasis() for the matrix, since 
@@ -318,7 +407,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
 					(invMass + lateral.dot(temp.cross(local2)));
 
 				MT_Scalar friction = MT_min(impulse_lateral, max_friction);
-				applyImpulse(local2 + getPosition(), -lateral * friction);
+				applyImpulse(local2 + getNextFrame().getPosition(), -lateral * friction);
 			}
 			else {
 				MT_Scalar impulse_lateral = rel_vel_lateral / invMass;
@@ -399,8 +488,8 @@ DT_Bool SM_Object::boing(
 	    (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback()))
 		scene->notifyCollision(obj1, obj2);
 	
-	local1 -= obj1->getPosition();
-	local2 -= obj2->getPosition();
+	local1 -= obj1->getNextFrame().getPosition();
+	local2 -= obj2->getNextFrame().getPosition();
 	
 	// Calculate collision parameters
 	MT_Vector3 rel_vel        = obj1->getVelocity(local1) - obj2->getVelocity(local2);
@@ -422,7 +511,11 @@ DT_Bool SM_Object::boing(
 		obj1->dynamicCollision(local1, normal, dist, rel_vel, restitution, friction_factor, invMass);
 		
 	if (obj2->isDynamic())
+	{
 		obj2->dynamicCollision(local2, -normal, dist, -rel_vel, restitution, friction_factor, invMass);
+		if (!obj1->isDynamic() || obj1->m_static)
+			obj2->m_static = obj1->m_static + 1;
+	}
 	
 	return DT_CONTINUE;
 }
@@ -466,31 +559,38 @@ DT_Bool SM_Object::fix(
 	if (dist < MT_EPSILON || dist > obj2->m_shapeProps->m_radius*obj2->m_shapeProps->m_radius)
 		return DT_CONTINUE;
 		
-	// This distinction between dynamic and non-dynamic objects should not be 
-	// necessary. Non-dynamic objects are assumed to have infinite mass.
-	if (obj1->isDynamic()) {
-		MT_Vector3 error = normal * 0.5f;
-		obj1->m_error += error;
-		obj2->m_error -= error;
-	}
-	else {
-		// Same again but now obj1 is non-dynamic
-		obj2->m_error -= normal;
+		
+	if ((obj1->m_static || !obj1->isDynamic()) && obj1->m_static < obj2->m_static)
+	{
+		obj2->m_static = obj1->m_static + 1;
+	} else if (obj2->m_static && obj2->m_static < obj1->m_static)
+	{
+		obj1->m_static = obj2->m_static + 1;
 	}
 	
+	contacts.insert(new Contact(obj1, obj2, normal, MT_Point3(local1 + 0.5*(local2 - local1))));
+	
+	
 	return DT_CONTINUE;
 }
 
 void SM_Object::relax(void)
-{ 
+{
+	for (Contact::Set::iterator csit = contacts.begin() ; csit != contacts.end(); ++csit)
+	{
+		(*csit)->resolve();
+		delete (*csit);
+	}
+		
+	contacts.clear();
 	if (m_error.fuzzyZero())
 		return;
 	//std::cout << "SM_Object::relax: { " << m_error << " }" << std::endl;
 	
-	setPosition(getPosition() + m_error); 
+	getNextFrame().setPosition(getNextFrame().getPosition() + m_error); 
 	m_error.setValue(0., 0., 0.); 
-	calcXform();
-	notifyClient();
+	//calcXform();
+	//notifyClient();
 }
 	
 SM_Object::SM_Object() :
@@ -636,15 +736,15 @@ calcXform() {
 	printf("                 m_scaling = { %-0.5f, %-0.5f, %-0.5f }\n",
 		m_scaling[0], m_scaling[1], m_scaling[2]);
 #endif
-	m_xform.setOrigin(getPosition());
-	m_xform.setBasis(MT_Matrix3x3(getOrientation(), m_scaling));
+	m_xform.setOrigin(getNextFrame().getPosition());
+	m_xform.setBasis(MT_Matrix3x3(getNextFrame().getOrientation(), m_scaling));
 	m_xform.getValue(m_ogl_matrix);
 	
 	/* Blender has been known to crash here.
 	   This usually means SM_Object *this has been deleted more than once. */
 	DT_SetMatrixd(m_object, m_ogl_matrix);
 	if (m_fh_object) {
-		m_fh_object->setPosition(getPosition());
+		m_fh_object->setPosition(getNextFrame().getPosition());
 		m_fh_object->calcXform();
 	}
 	updateInvInertiaTensor();
@@ -760,7 +860,8 @@ setPosition(
 	const MT_Point3& pos
 ){
 	m_kinematic = true;
-	SM_MotionState::setPosition(pos);
+	getNextFrame().setPosition(pos);
+	endFrame();
 }
 	
 	void 
@@ -770,7 +871,8 @@ setOrientation(
 ){
 	assert(!orn.fuzzyZero());
 	m_kinematic = true;
-	SM_MotionState::setOrientation(orn);
+	getNextFrame().setOrientation(orn);
+	endFrame();
 }
 
 	void 
@@ -807,10 +909,7 @@ SM_Object::
 addLinearVelocity(
 	const MT_Vector3& lin_vel
 ){
-	m_lin_vel += lin_vel;
-	if (m_shapeProps) {
-		m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
-	}
+	setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel);
 }
 
 	void 
@@ -818,9 +917,9 @@ SM_Object::
 setLinearVelocity(
 	const MT_Vector3& lin_vel
 ){
-	m_lin_vel = lin_vel;
+	getNextFrame().setLinearVelocity(lin_vel);
 	if (m_shapeProps) {
-		m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
+		m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass;
 	}
 }
 
@@ -849,9 +948,9 @@ SM_Object::
 setAngularVelocity(
 	const MT_Vector3& ang_vel
 ) {
-	m_ang_vel = ang_vel;
+	getNextFrame().setAngularVelocity(ang_vel);
 	if (m_shapeProps) {
-		m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
+		m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia;
 	}
 }
 
@@ -860,10 +959,7 @@ SM_Object::
 addAngularVelocity(
 	const MT_Vector3& ang_vel
 ) {
-	m_ang_vel += ang_vel;
-	if (m_shapeProps) {
-		m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
-	}
+	setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel);
 }
 
 
@@ -896,18 +992,18 @@ resolveCombinedVelocities(
 	if (isDynamic()) {		
 
 #if 1
-		m_lin_vel += lin_vel;
-		m_ang_vel += ang_vel;
+		getNextFrame().setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel);
+		getNextFrame().setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel);
 #else
 
 		//compute the component of the physics velocity in the 
 		// direction of the set velocity and set it to zero.
 		MT_Vector3 lin_vel_norm = lin_vel.normalized();
 
-		m_lin_vel -= (m_lin_vel.dot(lin_vel_norm) * lin_vel_norm);
+		setLinearVelocity(getNextFrame().getLinearVelocity() - (getNextFrame().getLinearVelocity().dot(lin_vel_norm) * lin_vel_norm));
 #endif
-		m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
-		m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
+		m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass;
+		m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia;
 		clearCombinedVelocities();
 
 	}
@@ -970,7 +1066,7 @@ applyImpulse(
 	const MT_Point3& attach, const MT_Vector3& impulse
 ) {
 	applyCenterImpulse(impulse);                          // Change in linear momentum
-	applyAngularImpulse((attach - m_pos).cross(impulse)); // Change in angular momentump
+	applyAngularImpulse((attach - getNextFrame().getPosition()).cross(impulse)); // Change in angular momentump
 }
 
 	void 
@@ -981,7 +1077,7 @@ applyCenterImpulse(
 	if (m_shapeProps) {
 		m_lin_mom          += impulse;
 		m_reaction_impulse += impulse;
-		m_lin_vel           = m_lin_mom * m_inv_mass;
+		getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass);
 
 		// The linear velocity is immedialtely updated since otherwise
 		// simultaneous collisions will get a double impulse. 
@@ -995,7 +1091,7 @@ applyAngularImpulse(
 ) {
 	if (m_shapeProps) {
 		m_ang_mom += impulse;
-		m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
+		getNextFrame().setAngularVelocity( m_inv_inertia_tensor * m_ang_mom);
 	}
 }
 
@@ -1099,7 +1195,7 @@ const
 SM_Object::
 actualLinVelocity(
 ) const {
-	return m_combined_lin_vel + m_lin_vel;
+	return m_combined_lin_vel + getNextFrame().getLinearVelocity();
 };
 
 const 
@@ -1107,10 +1203,98 @@ const
 SM_Object::
 actualAngVelocity(
 ) const {
-	return m_combined_ang_vel + m_ang_vel;
-};
+	return m_combined_ang_vel + getNextFrame().getAngularVelocity();
+}
+
+
+SM_MotionState&
+SM_Object::
+getCurrentFrame()
+{
+	return m_frames[1];
+}
+
+SM_MotionState&
+SM_Object::
+getPreviousFrame()
+{
+	return m_frames[0];
+}
+
+SM_MotionState &
+SM_Object::
+getNextFrame()
+{
+	return m_frames[2];
+}
+
+const SM_MotionState &
+SM_Object::
+getCurrentFrame() const
+{
+	return m_frames[1];
+}
+
+const SM_MotionState &
+SM_Object::
+getPreviousFrame() const
+{
+	return m_frames[0];
+}
+
+const SM_MotionState &
+SM_Object::
+getNextFrame() const
+{
+	return m_frames[2];
+}
 
 
+const MT_Point3&     
+SM_Object::
+getPosition()        const
+{
+	return m_frames[1].getPosition();
+}
 
+const MT_Quaternion& 
+SM_Object::
+getOrientation()     const
+{
+	return m_frames[1].getOrientation();
+}
 
+const MT_Vector3&    
+SM_Object::
+getLinearVelocity()  const
+{
+	return m_frames[1].getLinearVelocity();
+}
 
+const MT_Vector3&    
+SM_Object::
+getAngularVelocity() const
+{
+	return m_frames[1].getAngularVelocity();
+}
+
+void
+SM_Object::
+interpolate(MT_Scalar timeStep)
+{
+	if (!actualLinVelocity().fuzzyZero() || !actualAngVelocity().fuzzyZero()) 
+	{
+		getCurrentFrame().setTime(timeStep);
+		getCurrentFrame().lerp(getPreviousFrame(), getNextFrame());
+		notifyClient();
+	}
+}
+
+void
+SM_Object::
+endFrame()
+{
+	getPreviousFrame() = getNextFrame();
+	getCurrentFrame() = getNextFrame();
+	m_static = 0;
+}
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
index c29be228d64..e9f25e25e6c 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
+++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
@@ -54,7 +54,7 @@ SM_Scene::SM_Scene() :
 	m_secondaryRespTable(DT_CreateRespTable()),
 	m_fixRespTable(DT_CreateRespTable()),
 	m_forceField(0.0, 0.0, 0.0),
-	m_lastTime(-1.0)
+	m_frames(0)
 {
 	for (int i = 0 ; i < NUM_RESPONSE; i++)
 	{
@@ -83,8 +83,8 @@ SM_Scene::SM_Scene() :
 	
 	/* Fh Object */
 	DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
 	DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
 	
 	/* Object (Fix Pass) */
@@ -177,27 +177,31 @@ void SM_Scene::endFrame()
 		(*i)->clearForce();
 }
 
-bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
-	if (m_lastTime < 0.0)
+bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) 
+{
+	if (!m_frames)
 	{
-		m_lastTime = curtime;
-		return false;
+		if (ticrate > 0.)
+			m_frames = (unsigned int)(curtime*ticrate + 1.0);
+		else
+			m_frames = (unsigned int)(curtime*65536.0);
 	}
-		
+	
 	// Divide the timeStep into a number of subsamples of size roughly 
 	// equal to subS (might be a little smaller).
-	MT_Scalar timeStep = curtime - m_lastTime;
 	MT_Scalar subStep;
 	int num_samples;
 	
+	// Compute the number of steps to do this update.
 	if (ticrate > 0.0)
 	{
+		// Fixed time step
 		subStep = 1.0/ticrate;
-		num_samples = int(timeStep * ticrate);
+		num_samples = (unsigned int)(curtime*ticrate + 1.0) - m_frames;
 	
 		if (num_samples > 4)
 		{
-			std::cout << "Dropping physics frames! step: " << timeStep << " frames:" << num_samples << std::endl;
+			std::cout << "Dropping physics frames! frames:" << num_samples << std::endl;
 			num_samples /= 4;
 			subStep *= 4.0;
 		}
@@ -206,76 +210,70 @@ bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
 	{
 		// Variable time step. (old update)
 		// Integrate at least 100 Hz
+		MT_Scalar timeStep = curtime - m_frames/65536.0;
 		subStep = timeStep > 0.01 ? 0.01 : timeStep;
 		num_samples = int(timeStep * 0.01);
 		if (num_samples < 1)
 			num_samples = 1;
 	}
 	
+	// Do a physics timestep.
 	T_ObjectList::iterator i;
-	
-	// No timestep! (should do a mini update)
-	if (num_samples <= 0)
+	if (num_samples > 0)
 	{
-		// Apply a forcefield (such as gravity)
-#if 0
-		for (i = m_objectList.begin(); i != m_objectList.end(); ++i) 
+		// Do the integration steps per object.
+		for (int step = 0; step != num_samples; ++step) 
 		{
-			//(*i)->applyForceField(m_forceField);
-			//(*i)->integrateForces(timeStep);
-			// And second we update the object positions by performing
-			// an integration step for each object
-			(*i)->integrateMomentum(timeStep);
-			//(*i)->clearForce();
-		}
-#endif
-		return false;	
-	}
-	
-	m_lastTime += MT_Scalar(num_samples)*subStep;
+			MT_Scalar time;
+			if (ticrate > 0.)
+				time = MT_Scalar(m_frames + step + 1) * subStep;
+			else
+				time = MT_Scalar(m_frames)/65536.0 + MT_Scalar(step + 1)*subStep;
+			
+			for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
+				(*i)->endFrame();
+				// Apply a forcefield (such as gravity)
+				(*i)->integrateForces(subStep);
+				// And second we update the object positions by performing
+				// an integration step for each object
+				(*i)->integrateMomentum(subStep);
+			}
 	
-	// Do the integration steps per object.
-	int step;
-	for (step = 0; step != num_samples; ++step) {
-
-		for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
-			// Apply a forcefield (such as gravity)
-			//(*i)->applyForceField(m_forceField);
-			//(*i)->setTimeStep(timeStep);
-			(*i)->integrateForces(subStep);
-			// And second we update the object positions by performing
-			// an integration step for each object
-			(*i)->integrateMomentum(subStep);
-		}
+			// So now first we let the physics scene respond to 
+			// new forces, velocities set externally. 
+			// The collsion and friction impulses are computed here. 
+			// Collision phase
+			DT_Test(m_scene, m_respTable);
 		
-		// I changed the order of the next 2 statements.
-		// Originally objects were first integrated with a call
-		// to proceed(). However if external objects were 
-		// directly manipulating the velocities etc of physics 
-		// objects then the physics environment would not be able 
-		// to react before object positions were updated. --- Laurence.
-
-		// So now first we let the physics scene respond to 
-		// new forces, velocities set externally. 
-		// The collsion and friction impulses are computed here. 
-		DT_Test(m_scene, m_respTable);
-
-	// clear the user set velocities.
-#if 0
-	clearObjectCombinedVelocities();
-#endif
-		DT_Test(m_scene, m_fixRespTable);
-		
-		// Finish this timestep by saving al state information for the next
-		// timestep and clearing the accumulated forces. 
-		for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
-			(*i)->relax();
-			(*i)->proceedKinematic(subStep);
-			(*i)->saveReactionForce(subStep);
-			//(*i)->clearForce();
+			// Contact phase
+			DT_Test(m_scene, m_fixRespTable);
+			
+			// Finish this timestep by saving al state information for the next
+			// timestep and clearing the accumulated forces. 
+			for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
+				(*i)->relax();
+				(*i)->proceedKinematic(subStep);
+				(*i)->saveReactionForce(subStep);
+				(*i)->getNextFrame().setTime(time);
+				//(*i)->clearForce();
+			}
 		}
 	}
-	return true;
+
+	if (ticrate > 0)
+	{
+		// Interpolate between time steps.
+		for (i = m_objectList.begin(); i != m_objectList.end(); ++i) 
+			(*i)->interpolate(curtime);
+	
+		m_frames = (unsigned int)(curtime*ticrate + 1.0);
+	}
+	else
+	{
+		m_frames = (unsigned int)(curtime*65536.0);
+	}
+		
+	return num_samples != 0;
 }
 
 void SM_Scene::notifyCollision(SM_Object *obj1, SM_Object *obj2)