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.

11 files changed, 137 insertions, 33 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());