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.

20 files changed, 253 insertions, 85 deletions

diff --git a/source/gameengine/Ketsji/KX_ClientObjectInfo.h b/source/gameengine/Ketsji/KX_ClientObjectInfo.h
index 0ef1a1b86fe..237ec137a1c 100644
--- a/source/gameengine/Ketsji/KX_ClientObjectInfo.h
+++ b/source/gameengine/Ketsji/KX_ClientObjectInfo.h
@@ -32,10 +32,16 @@
 #ifndef __KX_CLIENTOBJECT_INFO_H
 #define __KX_CLIENTOBJECT_INFO_H
 
+#include <SM_Object.h>
+
+#include <list>
+
+class SCA_ISensor;
+class KX_GameObject;
 /**
  * Client Type and Additional Info. This structure can be use instead of a bare void* pointer, for safeness, and additional info for callbacks
  */
-struct KX_ClientObjectInfo
+struct KX_ClientObjectInfo : public SM_ClientObject
 {
 	enum clienttype {
 		STATIC,
@@ -44,15 +50,32 @@ struct KX_ClientObjectInfo
 		RADAR,
 		NEAR
 	}		m_type;
-	void*		m_clientobject;
+	KX_GameObject*	m_gameobject;
 	void*		m_auxilary_info;
+	std::list<SCA_ISensor*>	m_sensors;
 public:
-	KX_ClientObjectInfo(void *clientobject, clienttype type = STATIC, void *auxilary_info = NULL) :
+	KX_ClientObjectInfo(KX_GameObject *gameobject, clienttype type = STATIC, void *auxilary_info = NULL) :
+		SM_ClientObject(),
 		m_type(type),
-		m_clientobject(clientobject),
+		m_gameobject(gameobject),
 		m_auxilary_info(auxilary_info)
 	{}
 	
+	KX_ClientObjectInfo(const KX_ClientObjectInfo &copy)
+		: SM_ClientObject(copy),
+		  m_type(copy.m_type),
+		  m_gameobject(copy.m_gameobject),
+		  m_auxilary_info(copy.m_auxilary_info)
+	{
+	}
+	
+	virtual ~KX_ClientObjectInfo() {}
+	
+	virtual bool hasCollisionCallback() 
+	{
+		return m_sensors.size() != 0;
+	}
+	
 	bool isActor() { return m_type <= ACTOR; }
 };
 
diff --git a/source/gameengine/Ketsji/KX_GameObject.cpp b/source/gameengine/Ketsji/KX_GameObject.cpp
index 67148114de8..896da63ea04 100644
--- a/source/gameengine/Ketsji/KX_GameObject.cpp
+++ b/source/gameengine/Ketsji/KX_GameObject.cpp
@@ -188,7 +188,7 @@ void KX_GameObject::ProcessReplica(KX_GameObject* replica)
 	replica->m_pPhysicsController1 = NULL;
 	replica->m_pSGNode = NULL;
 	replica->m_pClient_info = new KX_ClientObjectInfo(*m_pClient_info);
-	replica->m_pClient_info->m_clientobject = replica;
+	replica->m_pClient_info->m_gameobject = replica;
 }
 
 
diff --git a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
index fbe2fd9c6cd..04777bf4a36 100644
--- a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
+++ b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
@@ -315,17 +315,13 @@ void KX_KetsjiEngine::NextFrame()
 	m_logger->StartLog(tc_services, m_kxsystem->GetTimeInSeconds(), true);
 
 	double curtime;
+	double localtime = m_previoustime;
 	if (m_bFixedTime)
-	{
-		m_deltatime = 1.0/m_ticrate;
 		curtime = m_previoustime + m_deltatime;
-	}
 	else
-	{
 		curtime = m_kxsystem->GetTimeInSeconds();
-		m_deltatime += curtime - m_previoustime;
-		m_previoustime = curtime;
-	}
+	m_deltatime += curtime - m_previoustime;
+	m_previoustime = curtime;
 
 	// Compute the number of logic frames to do each update (fixed tic bricks)
 	int frames = (int) (m_deltatime*m_ticrate);
@@ -337,13 +333,17 @@ void KX_KetsjiEngine::NextFrame()
 	
 	if (!frames)
 	{
-		for (sceneit = m_scenes.begin();sceneit != m_scenes.end(); sceneit++)
+		for (sceneit = m_scenes.begin();sceneit != m_scenes.end(); ++sceneit)
 		// for each scene, call the proceed functions
 		{
 			KX_Scene* scene = *sceneit;
 	
 			if (!scene->IsSuspended())
 			{
+				// set Python hooks for each scene
+				PHY_SetActiveEnvironment(scene->GetPhysicsEnvironment());
+				PHY_SetActiveScene(scene);
+				
 				// Do some cleanup work for this logic frame
 				m_logger->StartLog(tc_logic, m_kxsystem->GetTimeInSeconds(), true);
 				scene->LogicUpdateFrame(curtime, false);
@@ -356,7 +356,6 @@ void KX_KetsjiEngine::NextFrame()
 				// many iterations of the physics solver.
 				m_logger->StartLog(tc_physics, m_kxsystem->GetTimeInSeconds(), true);
 				scene->GetPhysicsEnvironment()->proceed(curtime);
-		
 				// Update scenegraph after physics step. This maps physics calculations
 				// into node positions.		
 				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
@@ -371,7 +370,8 @@ void KX_KetsjiEngine::NextFrame()
 	
 	while (frames--)
 	{
-		for (sceneit = m_scenes.begin();sceneit != m_scenes.end(); sceneit++)
+		localtime += 1.0/m_ticrate;
+		for (sceneit = m_scenes.begin();sceneit != m_scenes.end(); ++sceneit)
 		// for each scene, call the proceed functions
 		{
 			KX_Scene* scene = *sceneit;
@@ -385,6 +385,10 @@ void KX_KetsjiEngine::NextFrame()
 	
 			if (!scene->IsSuspended())
 			{
+				m_logger->StartLog(tc_physics, m_kxsystem->GetTimeInSeconds(), true);
+				scene->GetPhysicsEnvironment()->endFrame();
+				scene->GetPhysicsEnvironment()->beginFrame();
+			
 				m_logger->StartLog(tc_network, m_kxsystem->GetTimeInSeconds(), true);
 				scene->GetNetworkScene()->proceed(curtime);
 	
@@ -416,7 +420,6 @@ void KX_KetsjiEngine::NextFrame()
 				// many iterations of the physics solver.
 				m_logger->StartLog(tc_physics, m_kxsystem->GetTimeInSeconds(), true);
 				scene->GetPhysicsEnvironment()->proceed(curtime);
-		
 				// Update scenegraph after physics step. This maps physics calculations
 				// into node positions.		
 				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
diff --git a/source/gameengine/Ketsji/KX_MouseFocusSensor.cpp b/source/gameengine/Ketsji/KX_MouseFocusSensor.cpp
index 84b08058ecc..9380ef6f2fa 100644
--- a/source/gameengine/Ketsji/KX_MouseFocusSensor.cpp
+++ b/source/gameengine/Ketsji/KX_MouseFocusSensor.cpp
@@ -42,7 +42,6 @@
 #endif
 
 #include "MT_Point3.h"
-#include "KX_ClientObjectInfo.h"
 #include "RAS_FramingManager.h"
 #include "RAS_ICanvas.h"
 #include "RAS_IRasterizer.h"
@@ -51,11 +50,11 @@
 #include "KX_Camera.h"
 #include "KX_MouseFocusSensor.h"
 
-#include "KX_ClientObjectInfo.h"
 #include "SM_Object.h"
 #include "SM_Scene.h"
 #include "SumoPhysicsEnvironment.h"
 #include "KX_SumoPhysicsController.h"
+#include "KX_ClientObjectInfo.h"
 
 /* ------------------------------------------------------------------------- */
 /* Native functions                                                          */
@@ -259,7 +258,7 @@ bool KX_MouseFocusSensor::ParentObjectHasFocus(void)
 		/* all this casting makes me nervous... */
 		KX_ClientObjectInfo* client_info 
 			= ( hitSMObj ?
-				(KX_ClientObjectInfo*) hitSMObj->getClientObject() :
+				static_cast<KX_ClientObjectInfo*>( hitSMObj->getClientObject() ):
 				NULL);
 		
 		if (!client_info)
@@ -268,7 +267,7 @@ bool KX_MouseFocusSensor::ParentObjectHasFocus(void)
 			return false;
 		} 
 	
-		KX_GameObject* hitKXObj = (KX_GameObject*)client_info->m_clientobject;
+		KX_GameObject* hitKXObj = client_info->m_gameobject;
 		
 		if (client_info->m_type > KX_ClientObjectInfo::ACTOR)
 		{
diff --git a/source/gameengine/Ketsji/KX_NearSensor.cpp b/source/gameengine/Ketsji/KX_NearSensor.cpp
index 3bb6bad5029..bfcf9794d1b 100644
--- a/source/gameengine/Ketsji/KX_NearSensor.cpp
+++ b/source/gameengine/Ketsji/KX_NearSensor.cpp
@@ -62,7 +62,9 @@ KX_NearSensor::KX_NearSensor(SCA_EventManager* eventmgr,
 			 m_ResetMargin(resetmargin)
 
 {
+	gameobj->getClientInfo()->m_sensors.remove(this);
 	m_client_info = new KX_ClientObjectInfo(gameobj, KX_ClientObjectInfo::NEAR);
+	m_client_info->m_sensors.push_back(this);
 	
 	DT_ShapeHandle shape = (DT_ShapeHandle) vshape;
 	m_sumoObj = new SM_Object(shape,NULL,NULL,NULL);
@@ -90,7 +92,9 @@ KX_NearSensor::KX_NearSensor(SCA_EventManager* eventmgr,
 			 m_ResetMargin(resetmargin)
 
 {
+	gameobj->getClientInfo()->m_sensors.remove(this);
 	m_client_info = new KX_ClientObjectInfo(gameobj, KX_ClientObjectInfo::NEAR);
+	m_client_info->m_sensors.push_back(this);
 	
 	m_sumoObj = new SM_Object(DT_NewSphere(0.0),NULL,NULL,NULL);
 	m_sumoObj->setMargin(m_Margin);
@@ -115,7 +119,7 @@ CValue* KX_NearSensor::GetReplica()
 	// this will copy properties and so on...
 	CValue::AddDataToReplica(replica);
 	
-	replica->m_client_info = new KX_ClientObjectInfo(m_client_info->m_clientobject, KX_ClientObjectInfo::NEAR);
+	replica->m_client_info = new KX_ClientObjectInfo(m_client_info->m_gameobject, KX_ClientObjectInfo::NEAR);
 	
 	replica->m_sumoObj = new SM_Object(DT_NewSphere(0.0),NULL,NULL,NULL);
 	replica->m_sumoObj->setMargin(m_Margin);
@@ -132,7 +136,8 @@ void KX_NearSensor::ReParent(SCA_IObject* parent)
 {
 	SCA_ISensor::ReParent(parent);
 	
-	m_client_info->m_clientobject = static_cast<KX_GameObject*>(parent); 
+	m_client_info->m_gameobject = static_cast<KX_GameObject*>(parent); 
+	m_client_info->m_sensors.push_back(this);
 	
 	SynchronizeTransform();
 }
@@ -197,7 +202,7 @@ DT_Bool KX_NearSensor::HandleCollision(void* obj1,void* obj2,const DT_CollData *
 					((SM_Object*)obj1)->getClientObject());
 
 	KX_GameObject* gameobj = ( client_info ? 
-			static_cast<KX_GameObject*>(client_info->m_clientobject) : 
+			client_info->m_gameobject :
 			NULL);
 	
 	if (gameobj && (gameobj != parent))
diff --git a/source/gameengine/Ketsji/KX_RadarSensor.cpp b/source/gameengine/Ketsji/KX_RadarSensor.cpp
index 798ebdfc944..1ad37504224 100644
--- a/source/gameengine/Ketsji/KX_RadarSensor.cpp
+++ b/source/gameengine/Ketsji/KX_RadarSensor.cpp
@@ -88,7 +88,7 @@ CValue* KX_RadarSensor::GetReplica()
 	// this will copy properties and so on...
 	CValue::AddDataToReplica(replica);
 	
-	replica->m_client_info = new KX_ClientObjectInfo(m_client_info->m_clientobject, KX_ClientObjectInfo::RADAR);
+	replica->m_client_info = new KX_ClientObjectInfo(m_client_info->m_gameobject, KX_ClientObjectInfo::RADAR);
 	
 	replica->m_sumoObj = new SM_Object(DT_NewCone(m_coneradius, m_coneheight),NULL,NULL,NULL);
 	replica->m_sumoObj->setMargin(m_Margin);
diff --git a/source/gameengine/Ketsji/KX_RaySensor.cpp b/source/gameengine/Ketsji/KX_RaySensor.cpp
index 04f57bb754b..c60b2171cab 100644
--- a/source/gameengine/Ketsji/KX_RaySensor.cpp
+++ b/source/gameengine/Ketsji/KX_RaySensor.cpp
@@ -195,7 +195,7 @@ bool KX_RaySensor::Evaluate(CValue* event)
 		if (hitObj)
 		{
 	
-			KX_ClientObjectInfo* info = (KX_ClientObjectInfo*)hitObj->getClientObject();
+			KX_ClientObjectInfo* info = static_cast<KX_ClientObjectInfo*>(hitObj->getClientObject());
 			bool bFound = false;
 			
 			if (!info)
@@ -205,7 +205,7 @@ bool KX_RaySensor::Evaluate(CValue* event)
 				break;
 			} 
 			
-			SCA_IObject *hitgameobj = (SCA_IObject*)info->m_clientobject;
+			SCA_IObject *hitgameobj = info->m_gameobject;
 			
 			if (hitgameobj == obj || info->m_type > KX_ClientObjectInfo::ACTOR)
 			{
diff --git a/source/gameengine/Ketsji/KX_SumoPhysicsController.cpp b/source/gameengine/Ketsji/KX_SumoPhysicsController.cpp
index a5284b40165..8f284809a17 100644
--- a/source/gameengine/Ketsji/KX_SumoPhysicsController.cpp
+++ b/source/gameengine/Ketsji/KX_SumoPhysicsController.cpp
@@ -8,6 +8,7 @@
 #include "SM_Scene.h"
 #include "KX_GameObject.h"
 #include "KX_MotionState.h"
+#include "KX_ClientObjectInfo.h"
 
 #ifdef HAVE_CONFIG_H
 #include <config.h>
diff --git a/source/gameengine/Ketsji/KX_TouchEventManager.cpp b/source/gameengine/Ketsji/KX_TouchEventManager.cpp
index 704d949b0e7..4f074ce9b12 100644
--- a/source/gameengine/Ketsji/KX_TouchEventManager.cpp
+++ b/source/gameengine/Ketsji/KX_TouchEventManager.cpp
@@ -40,6 +40,31 @@
 
 #include "SM_Object.h"
 
+KX_TouchEventManager::Collision::Collision(SCA_ISensor *sensor, SM_Object *obj1, SM_Object *obj2)
+	: m_id(next_id++),
+	  m_sensor(sensor),
+	  m_object1(obj1),
+	  m_object2(obj2)
+{
+}
+
+bool KX_TouchEventManager::Collision::operator<(const Collision &other) const
+{
+	if (*this == other)
+		return true;
+		
+	return m_id < other.m_id;
+}
+		
+bool KX_TouchEventManager::Collision::operator==(const Collision &other) const
+{
+	return m_sensor == other.m_sensor && 
+		((m_object1 == other.m_object1 && m_object2 == other.m_object2) ||
+		 (m_object1 == other.m_object2 && m_object2 == other.m_object1));
+}
+
+int KX_TouchEventManager::Collision::next_id = 0;
+
 KX_TouchEventManager::KX_TouchEventManager(class SCA_LogicManager* logicmgr,
 	SM_Scene *scene)
 	: SCA_EventManager(TOUCH_EVENTMGR),
@@ -51,6 +76,24 @@ KX_TouchEventManager::KX_TouchEventManager(class SCA_LogicManager* logicmgr,
 	m_scene->addTouchCallback(SENSOR_RESPONSE, KX_TouchEventManager::collisionResponse, this);
 }
 
+DT_Bool KX_TouchEventManager::HandleCollision(void* object1, void* object2, const DT_CollData *coll_data)
+{
+	SM_Object * obj1 = static_cast<SM_Object*>(object1);
+	SM_Object * obj2 = static_cast<SM_Object*>(object2);
+	KX_ClientObjectInfo *client_info = static_cast<KX_ClientObjectInfo *>(obj1->getClientObject());
+	
+	list<SCA_ISensor*>::iterator it;
+	for ( it = client_info->m_sensors.begin(); it != client_info->m_sensors.end(); ++it)
+		m_collisions.insert(Collision(*it, obj1, obj2));
+
+	client_info = static_cast<KX_ClientObjectInfo *>(obj2->getClientObject());
+	for ( it = client_info->m_sensors.begin(); it != client_info->m_sensors.end(); ++it)
+		m_collisions.insert(Collision(*it, obj2, obj1));
+		
+	return DT_CONTINUE;
+}
+
+/*
 DT_Bool KX_TouchEventManager::HandleCollision(void* object1,void* object2,
 						 const DT_CollData * coll_data)
 {
@@ -76,6 +119,8 @@ DT_Bool KX_TouchEventManager::HandleCollision(void* object1,void* object2,
 	return DT_CONTINUE;
 }
 
+*/
+
 DT_Bool KX_TouchEventManager::collisionResponse(void *client_data, 
 							void *object1,
 							void *object2,
@@ -115,10 +160,15 @@ void KX_TouchEventManager::NextFrame()
 	{
 		vector<SCA_ISensor*>::iterator it;
 		
-		for (it = m_sensors.begin();!(it==m_sensors.end());it++)
+		for (it = m_sensors.begin();!(it==m_sensors.end());++it)
 			static_cast<KX_TouchSensor*>(*it)->SynchronizeTransform();
 		
-		for (it = m_sensors.begin();!(it==m_sensors.end());it++)
+		for (std::set<Collision>::iterator cit = m_collisions.begin(); cit != m_collisions.end(); ++cit)
+			static_cast<KX_TouchSensor*>((*cit).m_sensor)->HandleCollision((*cit).m_object1, (*cit).m_object2, NULL);
+			
+		m_collisions.clear();
+			
+		for (it = m_sensors.begin();!(it==m_sensors.end());++it)
 			(*it)->Activate(m_logicmgr,NULL);
 	}
 }
diff --git a/source/gameengine/Ketsji/KX_TouchEventManager.h b/source/gameengine/Ketsji/KX_TouchEventManager.h
index 923ecd10884..eb2c26a64f3 100644
--- a/source/gameengine/Ketsji/KX_TouchEventManager.h
+++ b/source/gameengine/Ketsji/KX_TouchEventManager.h
@@ -37,11 +37,32 @@
 #include "KX_GameObject.h"
 
 #include <vector>
+#include <set>
+
+class SCA_ISensor;
+class SM_Object;
 
 class KX_TouchEventManager : public SCA_EventManager
 {
+	struct Collision
+	{
+		static int next_id;
+		int          m_id;
+		SCA_ISensor *m_sensor;
+		SM_Object   *m_object1;
+		SM_Object   *m_object2;
+		
+		Collision(SCA_ISensor *sensor, SM_Object *obj1, SM_Object *obj2);
+		
+		bool operator<(const Collision &other) const;
+		bool operator==(const Collision &other) const;
+	};
+
 	class SCA_LogicManager* m_logicmgr;
 	SM_Scene *m_scene;
+	
+	std::set<Collision> m_collisions;
+	
 
 	static DT_Bool collisionResponse(void *client_data, 
 							void *object1,
diff --git a/source/gameengine/Ketsji/KX_TouchSensor.cpp b/source/gameengine/Ketsji/KX_TouchSensor.cpp
index 9091a41d054..4a7e9d8646d 100644
--- a/source/gameengine/Ketsji/KX_TouchSensor.cpp
+++ b/source/gameengine/Ketsji/KX_TouchSensor.cpp
@@ -103,8 +103,9 @@ m_bLastTriggered(false)
 	m_colliders = new CListValue();
 	
 	KX_ClientObjectInfo *client_info = gameobj->getClientInfo();
-	client_info->m_clientobject = gameobj;
+	client_info->m_gameobject = gameobj;
 	client_info->m_auxilary_info = NULL;
+	client_info->m_sensors.push_back(this);
 	
 	KX_SumoPhysicsController *sphy = dynamic_cast<KX_SumoPhysicsController *>(gameobj->GetPhysicsController());
 	if (sphy)
@@ -141,8 +142,9 @@ void	KX_TouchSensor::ReParent(SCA_IObject* parent)
 
 //	m_solidHandle = m_sumoObj->getObjectHandle();
 	KX_ClientObjectInfo *client_info = gameobj->getClientInfo();
-	client_info->m_clientobject = parent;
+	client_info->m_gameobject = gameobj;
 	client_info->m_auxilary_info = NULL;
+	client_info->m_sensors.push_back(this);
 	SCA_ISensor::ReParent(parent);
 }
 
@@ -164,12 +166,12 @@ DT_Bool    KX_TouchSensor::HandleCollision(void* obj1,void* obj2,const DT_CollDa
 
 	// need the mapping from SM_Objects to gameobjects now
 	
-	KX_ClientObjectInfo* client_info =(KX_ClientObjectInfo*) (obj1 == m_sumoObj? 
+	KX_ClientObjectInfo* client_info = static_cast<KX_ClientObjectInfo*> (obj1 == m_sumoObj? 
 					((SM_Object*)obj2)->getClientObject() : 
 					((SM_Object*)obj1)->getClientObject());
 
 	KX_GameObject* gameobj = ( client_info ? 
-			(KX_GameObject*)client_info->m_clientobject : 
+			client_info->m_gameobject : 
 			NULL);
 	
 	if (gameobj && (gameobj != parent) && client_info->isActor())
@@ -338,7 +340,7 @@ PyObject* KX_TouchSensor::PyGetHitObjectList(PyObject* self,
 				SM_Object* smob = spc?spc->GetSumoObject():NULL;
 				
 				if (smob) {
-					KX_ClientObjectInfo* cl_inf = (KX_ClientObjectInfo*) smob->getClientObject();
+					KX_ClientObjectInfo* cl_inf = static_cast<KX_ClientObjectInfo*>(smob->getClientObject());
 					
 					if (m_touchedpropname == ((char*)cl_inf->m_auxilary_info)) {
 						newList->Add(m_colliders->GetValue(i)->AddRef());
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,