Speed up the physics engine: hook the SOLID broad phase, so we can either reject the test or request the penetration depth test as necessary. Previously we were doing the penetration depth test, as well as SOLID's intersection test.

9 files changed, 116 insertions, 52 deletions

diff --git a/source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.cpp b/source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.cpp
index bfb24f5fec1..6c69e14c886 100644
--- a/source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.cpp
@@ -48,14 +48,24 @@ DummyPhysicsEnvironment::~DummyPhysicsEnvironment()
 	//destroy physicsengine data
 }
 
-void DummyPhysicsEnvironment::proceed(double timeStep)
+void DummyPhysicsEnvironment::beginFrame()
+{
+	// beginning of logic frame: apply forces
+}
+
+void DummyPhysicsEnvironment::endFrame()
+{
+	// end of logic frame: clear forces
+}
+
+bool DummyPhysicsEnvironment::proceed(double timeStep)
 {
 	//step physics simulation, typically perform
 	
 	//collision detection 
 	//solve constraints
 	//integrate solution
-
+	// return true if an update was done.
 }
 
 void DummyPhysicsEnvironment::setGravity(float x,float y,float z)
diff --git a/source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.h b/source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.h
index 96f1ff82f9e..65f42141045 100644
--- a/source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.h
@@ -48,8 +48,10 @@ class DummyPhysicsEnvironment  : public PHY_IPhysicsEnvironment
 public:
 	DummyPhysicsEnvironment ();
 	virtual		~DummyPhysicsEnvironment ();
+	virtual void		beginFrame();
+	virtual void		endFrame();
 // Perform an integration step of duration 'timeStep'.
-	virtual	void		proceed (double	timeStep);
+	virtual	bool		proceed (double	timeStep);
 	virtual	void		setGravity(float x,float y,float z);
 
 	virtual int			createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
index 54574f9fc55..4a69f7e2990 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
+++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
@@ -66,6 +66,14 @@ struct SM_MaterialProps {
 	bool      m_fh_normal;             ///< Should the object slide off slopes?
 };
 
+class SM_ClientObject
+{
+public:
+	SM_ClientObject() {}
+	virtual ~SM_ClientObject() {}
+	
+	virtual bool hasCollisionCallback() = 0;
+};
 
 /**
  * SM_Object is an internal part of the Sumo physics engine.
@@ -227,7 +235,6 @@ public:
 
 	SM_Object *getDynamicParent() ;
 
-	void beginFrame();
 	void integrateForces(MT_Scalar timeStep);
 	void integrateMomentum(MT_Scalar timeSteo);
 
@@ -255,8 +262,8 @@ public:
 	);
 	
 	
-	void *getClientObject() { return m_client_object; }
-	void setClientObject(void *client_object) { m_client_object = client_object; }
+	SM_ClientObject *getClientObject() { return m_client_object; }
+	void setClientObject(SM_ClientObject *client_object) { m_client_object = client_object; }
 	
 	void relax();
 	
@@ -301,7 +308,7 @@ private:
     // as the collision callback now has only information
 	// on an SM_Object, there must be a way that the SM_Object client
 	// can identify it's clientdata after a collision
-	void                   *m_client_object;
+	SM_ClientObject        *m_client_object;
 
 	DT_ShapeHandle          m_shape;                 // Shape for collision detection
 
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h
index 1c11df66c38..0b429a3c1a4 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h
+++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h
@@ -101,13 +101,15 @@ public:
 	
 	void requestCollisionCallback(SM_Object &object);
 
+	void beginFrame();
+	void endFrame();
 
 	// Perform an integration step of duration 'timeStep'.
 	// 'subSampling' is the maximum duration of a substep, i.e.,
 	// The maximum time interval between two collision checks.
 	// 'subSampling' can be used to control aliasing effects
 	// (fast moving objects traversing through walls and such). 
-	void proceed(MT_Scalar curtime, MT_Scalar ticrate);
+	bool proceed(MT_Scalar curtime, MT_Scalar ticrate);
 
 	/**
 	 * Test whether any objects lie on the line defined by from and
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
index 940f0d221a0..42adfcd4d9e 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
+++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
@@ -101,16 +101,6 @@ SM_Object::SM_Object(
 	m_suspended = false;
 }
 
-	void
-SM_Object::
-beginFrame(
-){
-	if (!m_suspended) {
-		m_prev_state = *this;
-		m_prev_state.setLinearVelocity(actualLinVelocity());
-		m_prev_state.setAngularVelocity(actualAngVelocity());
-	}
-}
 
 	void 
 SM_Object::
@@ -118,6 +108,9 @@ integrateForces(
 	MT_Scalar timeStep
 ){
 	if (!m_suspended) {
+		m_prev_state = *this;
+		m_prev_state.setLinearVelocity(actualLinVelocity());
+		m_prev_state.setAngularVelocity(actualAngVelocity());
 		if (isDynamic()) {
 			// Integrate momentum (forward Euler)
 			m_lin_mom += m_force * timeStep;
@@ -348,49 +341,68 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
 	}
 }
 
+static void AddCallback(SM_Scene *scene, SM_Object *obj1, SM_Object *obj2)
+{
+	// If we have callbacks on either of the client objects, do a collision test
+	// and add a callback if they intersect.
+	DT_Vector3 v;
+	if ((obj1->getClientObject() && obj1->getClientObject()->hasCollisionCallback()) || 
+	    (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback()) &&
+	     DT_GetIntersect(obj1->getObjectHandle(), obj2->getObjectHandle(), v))
+		scene->addPair(obj1, obj2);
+}
+
 DT_Bool SM_Object::boing(
 	void *client_data,  
 	void *object1,
 	void *object2,
 	const DT_CollData *coll_data
 ){
-	//if (!coll_data)
-	//	return DT_CONTINUE;
-
 	SM_Scene  *scene = (SM_Scene *)client_data; 
 	SM_Object *obj1  = (SM_Object *)object1;  
 	SM_Object *obj2  = (SM_Object *)object2;  
 	
-	scene->addPair(obj1, obj2); // Record this collision for client callbacks
+	// at this point it is unknown whether we are really intersecting (broad phase)
+	
+	DT_Vector3 p1, p2;
+	if (!obj2->isDynamic()) {
+		std::swap(obj1, obj2);
+	}
 	
 	// If one of the objects is a ghost then ignore it for the dynamics
 	if (obj1->isGhost() || obj2->isGhost()) {
+		AddCallback(scene, obj1, obj2);
 		return DT_CONTINUE;
 	}
 
 	// Objects do not collide with parent objects
 	if (obj1->getDynamicParent() == obj2 || obj2->getDynamicParent() == obj1) {
+		AddCallback(scene, obj1, obj2);
 		return DT_CONTINUE;
 	}
 	
 	if (!obj2->isDynamic()) {
-		std::swap(obj1, obj2);
-	}
-
-	if (!obj2->isDynamic()) {
+		AddCallback(scene, obj1, obj2);
 		return DT_CONTINUE;
 	}
 
 	// Get collision data from SOLID
-	DT_Vector3 p1, p2;
 	if (!DT_GetPenDepth(obj1->getObjectHandle(), obj2->getObjectHandle(), p1, p2))
 		return DT_CONTINUE;
+	
 	MT_Point3 local1(p1), local2(p2);
 	MT_Vector3 normal(local2 - local1);
 	MT_Scalar dist = normal.length();
 	
 	if (dist < MT_EPSILON)
 		return DT_CONTINUE;
+		
+	// Now we are definately intersecting.
+
+	// Set callbacks for game engine.
+	if ((obj1->getClientObject() && obj1->getClientObject()->hasCollisionCallback()) || 
+	    (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback()))
+		scene->addPair(obj1, obj2);
 	
 	local1 -= obj1->m_pos;
 	local2 -= obj2->m_pos;
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
index 0af1c22edbd..1ea19c56336 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
+++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
@@ -65,38 +65,38 @@ SM_Scene::SM_Scene() :
 	
 	/* Sensor */
 	DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Scene::boing, DT_BROAD_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Scene::boing, DT_BROAD_RESPONSE, this);
 	DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
 	
 	/* Static */
-	DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_BROAD_RESPONSE, this);
 	DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[STATIC_RESPONSE], 0, DT_NO_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
 	
 	/* Object */
-	DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this);
-	DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_BROAD_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
+	DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
 	
 	/* 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_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[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[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
 	
 	/* Object (Fix Pass) */
 	DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
-	DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::fix, DT_SIMPLE_RESPONSE, this);
-	DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::fix, DT_SIMPLE_RESPONSE, this);
+	DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::fix, DT_BROAD_RESPONSE, this);
+	DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::fix, DT_BROAD_RESPONSE, this);
 	DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
 }
 
 void SM_Scene::addTouchCallback(int response_class, DT_ResponseCallback callback, void *user)
 {
-	DT_AddClassResponse(m_secondaryRespTable, m_secondaryResponseClass[response_class], callback, DT_SIMPLE_RESPONSE, user);
+	DT_AddClassResponse(m_secondaryRespTable, m_secondaryResponseClass[response_class], callback, DT_BROAD_RESPONSE, user);
 }
 
 void SM_Scene::addSensor(SM_Object& object) 
@@ -161,11 +161,27 @@ void SM_Scene::remove(SM_Object& object) {
 	}
 }
 
-void SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
+void SM_Scene::beginFrame()
+{
+	T_ObjectList::iterator i;
+	// Apply a forcefield (such as gravity)
+	for (i = m_objectList.begin(); i != m_objectList.end(); ++i)
+		(*i)->applyForceField(m_forceField);
+
+}
+
+void SM_Scene::endFrame()
+{
+	T_ObjectList::iterator i;
+	for (i = m_objectList.begin(); i != m_objectList.end(); ++i)
+		(*i)->clearForce();
+}
+
+bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
 	if (m_lastTime < 0.0)
 	{
 		m_lastTime = curtime;
-		return;
+		return false;
 	}
 		
 	// Divide the timeStep into a number of subsamples of size roughly 
@@ -177,7 +193,7 @@ void SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
 	T_ObjectList::iterator i;
 	
 	// No timestep! (should do a mini update)
-	if (!num_samples)
+	if (num_samples <= 0)
 	{
 		// Apply a forcefield (such as gravity)
 #if 0
@@ -191,7 +207,7 @@ void SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
 			//(*i)->clearForce();
 		}
 #endif
-		return;	
+		return false;	
 	}
 
 	m_lastTime += MT_Scalar(num_samples)*subStep;
@@ -201,9 +217,8 @@ void SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
 	for (step = 0; step != num_samples; ++step) {
 
 		for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
-			(*i)->beginFrame();
 			// Apply a forcefield (such as gravity)
-			(*i)->applyForceField(m_forceField);
+			//(*i)->applyForceField(m_forceField);
 			//(*i)->setTimeStep(timeStep);
 			(*i)->integrateForces(subStep);
 			// And second we update the object positions by performing
@@ -235,7 +250,7 @@ void SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
 			(*i)->relax();
 			(*i)->proceedKinematic(subStep);
 			(*i)->saveReactionForce(subStep);
-			(*i)->clearForce();
+			//(*i)->clearForce();
 		}
 	}
 	// For each pair of object that collided, call the corresponding callback.
@@ -252,6 +267,8 @@ void SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
 	}
 	
 	clearPairs();
+	
+	return true;
 }
 
 SM_Object *SM_Scene::rayTest(void *ignore_client, 
diff --git a/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp b/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp
index 151c55d75de..f035bfb4cf2 100644
--- a/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp
@@ -62,9 +62,19 @@ MT_Scalar SumoPhysicsEnvironment::getTicRate()
 	return PhysicsTicRate;
 }
 
-void SumoPhysicsEnvironment::proceed(double curtime)
+void SumoPhysicsEnvironment::beginFrame()
 {
-	m_sumoScene->proceed(curtime, PhysicsTicRate);
+	m_sumoScene->beginFrame();
+}
+
+void SumoPhysicsEnvironment::endFrame()
+{
+	m_sumoScene->endFrame();
+}
+
+bool SumoPhysicsEnvironment::proceed(double curtime)
+{
+	return m_sumoScene->proceed(curtime, PhysicsTicRate);
 }
 
 void SumoPhysicsEnvironment::setGravity(float x,float y,float z)
diff --git a/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.h b/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.h
index 09ffe6ce112..aba341360f9 100644
--- a/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.h
@@ -48,8 +48,10 @@ class SumoPhysicsEnvironment : public PHY_IPhysicsEnvironment
 public:
 	SumoPhysicsEnvironment();
 	virtual		~SumoPhysicsEnvironment();
+	virtual	void		beginFrame();
+	virtual void		endFrame();
 // Perform an integration step of duration 'timeStep'.
-	virtual	void		proceed(double curtime);
+	virtual	bool		proceed(double curtime);
 	virtual	void		setGravity(float x,float y,float z);
 	virtual int		createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
 			float pivotX,float pivotY,float pivotZ,
diff --git a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
index 1eabc4bfd64..0fdac35f514 100644
--- a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
+++ b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
@@ -43,8 +43,10 @@ class PHY_IPhysicsEnvironment
 {
 	public:
 		virtual		~PHY_IPhysicsEnvironment();
+		virtual	void		beginFrame() = 0;
+		virtual void		endFrame() = 0;
 		/// Perform an integration step of duration 'timeStep'.
-		virtual	void		proceed(double	timeStep)=0;
+		virtual	bool		proceed(double	timeStep)=0;
 		virtual	void		setGravity(float x,float y,float z)=0;
 
 		virtual int			createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,