Radar/Near sensor performance problem fixed

8 files changed, 146 insertions, 30 deletions

diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
index 5a45ce020cc..963dda10873 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
@@ -36,6 +36,7 @@ float gAngularSleepingTreshold = 1.0f;
 
 
 btVector3 startVel(0,0,0);//-10000);
+
 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
 :m_cci(ci)
 {
@@ -119,17 +120,20 @@ void CcdPhysicsController::CreateRigidbody()
 		m_cci.m_linearDamping,m_cci.m_angularDamping,
 		m_cci.m_friction,m_cci.m_restitution);
 
-	
-
 	//
 	// init the rigidbody properly
 	//
 	
 	//setMassProps this also sets collisionFlags
 	//convert collision flags!
-
+	//special case: a near/radar sensor controller should not be defined static or it will
+	//generate loads of static-static collision messages on the console
+	if ((m_cci.m_collisionFilterGroup & CcdConstructionInfo::SensorFilter) != 0)
+	{
+		// reset the flags that have been set so far
+		m_body->setCollisionFlags(0);
+	}
 	m_body->setCollisionFlags(m_body->getCollisionFlags() | m_cci.m_collisionFlags);
-	
 	m_body->setGravity( m_cci.m_gravity);
 	m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
 
@@ -141,12 +145,14 @@ CcdPhysicsController::~CcdPhysicsController()
 	if (m_cci.m_physicsEnv)
 		m_cci.m_physicsEnv->removeCcdPhysicsController(this);
 
-	delete m_MotionState;
+	if (m_MotionState)
+		delete m_MotionState;
 	if (m_bulletMotionState)
 		delete m_bulletMotionState;
 	delete m_body;
 }
 
+
 		/**
 			SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
 		*/
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
index 11fef56401f..54b4bcc40ee 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
@@ -46,7 +46,8 @@ struct CcdConstructionInfo
 	        StaticFilter = 2,
 	        KinematicFilter = 4,
 	        DebrisFilter = 8,
-	        AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter,
+			SensorFilter = 16,
+	        AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorFilter,
 	};
 
 
@@ -61,6 +62,7 @@ struct CcdConstructionInfo
 		m_collisionFlags(0),
 		m_collisionFilterGroup(DefaultFilter),
 		m_collisionFilterMask(AllFilter),
+		m_collisionShape(0),
 		m_MotionState(0),
 		m_physicsEnv(0),
 		m_inertiaFactor(1.f)
@@ -85,9 +87,8 @@ struct CcdConstructionInfo
 	short int	m_collisionFilterGroup;
 	short int	m_collisionFilterMask;
 
-
-	btCollisionShape*			m_collisionShape;
-	class	PHY_IMotionState*			m_MotionState;
+	class btCollisionShape*	m_collisionShape;
+	class PHY_IMotionState*	m_MotionState;
 	
 	CcdPhysicsEnvironment*	m_physicsEnv; //needed for self-replication
 	float	m_inertiaFactor;//tweak the inertia (hooked up to Blender 'formfactor'
@@ -96,11 +97,12 @@ struct CcdConstructionInfo
 
 class btRigidBody;
 
+
 ///CcdPhysicsController is a physics object that supports continuous collision detection and time of impact based physics resolution.
 class CcdPhysicsController : public PHY_IPhysicsController	
 {
 	btRigidBody* m_body;
-	class	PHY_IMotionState*			m_MotionState;
+	class PHY_IMotionState*		m_MotionState;
 	btMotionState* 	m_bulletMotionState;
 
 
@@ -215,7 +217,11 @@ class CcdPhysicsController : public PHY_IPhysicsController
 			return m_MotionState;
 		}
 
-
+		class CcdPhysicsEnvironment* GetPhysicsEnvironment()
+		{
+			return m_cci.m_physicsEnv;
+		}
+		
 };
 
 
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index 99c3e5f77c7..25defbd5587 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -251,6 +251,22 @@ public:
 };
 #endif //NEW_BULLET_VEHICLE_SUPPORT
 
+class CcdOverlapFilterCallBack : public btOverlapFilterCallback
+{
+private:
+	class CcdPhysicsEnvironment* m_physEnv;
+public:
+	CcdOverlapFilterCallBack(CcdPhysicsEnvironment* env) : 
+		m_physEnv(env)
+	{
+	}
+	virtual ~CcdOverlapFilterCallBack()
+	{
+	}
+	// return true when pairs need collision
+	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const;
+};
+
 
 void CcdPhysicsEnvironment::setDebugDrawer(btIDebugDraw* debugDrawer)
 {
@@ -302,7 +318,11 @@ m_numTimeSubSteps(1),
 m_ccdMode(0),
 m_solverType(-1),
 m_profileTimings(0),
-m_enableSatCollisionDetection(false)
+m_enableSatCollisionDetection(false),
+m_solver(NULL),
+m_ownPairCache(NULL),
+m_ownDispatcher(NULL),
+m_filterCallback(NULL)
 {
 
 	for (int i=0;i<PHY_NUM_RESPONSE;i++)
@@ -310,8 +330,10 @@ m_enableSatCollisionDetection(false)
 		m_triggerCallbacks[i] = 0;
 	}
 	if (!dispatcher)
+	{
 		dispatcher = new btCollisionDispatcher();
-
+		m_ownDispatcher = dispatcher;
+	}
 
 	if(!pairCache)
 	{
@@ -321,14 +343,16 @@ m_enableSatCollisionDetection(false)
 		btVector3 worldMax(10000,10000,10000);
 
 		pairCache = new btAxisSweep3(worldMin,worldMax);
-
+		// remember that this was allocated by us so that we can release it
+		m_ownPairCache = pairCache;
 		//broadphase = new btSimpleBroadphase();
 	}
 
+	m_filterCallback = new CcdOverlapFilterCallBack(this);
+	pairCache->setOverlapFilterCallback(m_filterCallback);
 
 	setSolverType(1);//issues with quickstep and memory allocations
-
-	m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,new btSequentialImpulseConstraintSolver());
+	m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,m_solver);
 	m_debugDrawer = 0;
 	m_gravity = btVector3(0.f,-10.f,0.f);
 	m_dynamicsWorld->setGravity(m_gravity);
@@ -346,7 +370,8 @@ void	CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
 	body->setGravity( m_gravity );
 	m_controllers.push_back(ctrl);
 
-	m_dynamicsWorld->addRigidBody(body);
+	//use explicit group/filter for finer control over collision in bullet => near/radar sensor
+	m_dynamicsWorld->addRigidBody(body, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
 	if (body->isStaticOrKinematicObject())
 	{
 		body->setActivationState(ISLAND_SLEEPING);
@@ -778,7 +803,20 @@ CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
 	delete m_dynamicsWorld;
 	
 
+	if (NULL != m_ownPairCache)
+		delete m_ownPairCache;
+
+	if (NULL != m_ownDispatcher)
+		delete m_ownDispatcher;
+
+	if (NULL != m_solver)
+		delete m_solver;
+
+	if (NULL != m_debugDrawer)
+		delete m_debugDrawer;
 
+	if (NULL != m_filterCallback)
+		delete m_filterCallback;
 }
 
 
@@ -841,9 +879,10 @@ void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
 	{
 		addCcdPhysicsController(ctrl1);
 	}
+	//Collision filter/mask is now set at the time of the creation of the controller 
 	//force collision detection with everything, including static objects (might hurt performance!)
-	ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
-	ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::AllFilter;
+	//ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::SensorTrigger;
+	//ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::SensorTrigger;
 	//todo: make this 'sensor'!
 
 	requestCollisionCallback(ctrl);
@@ -962,9 +1001,47 @@ void	CcdPhysicsEnvironment::CallbackTriggers()
 
 }
 
-
-
-
+// This call back is called before a pair is added in the cache
+// Handy to remove objects that must be ignored by sensors
+bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+{
+	btCollisionObject *colObj0, *colObj1;
+	CcdPhysicsController *sensorCtrl, *objCtrl;
+	bool collides;
+	// first check the filters
+	collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+	collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+	if (!collides)
+		return false;
+
+	// additional check for sensor object
+	if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
+	{
+		// this is a sensor object, the other one can't be a sensor object because 
+		// they exclude each other in the above test
+		assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
+		colObj0 = (btCollisionObject*)proxy0->m_clientObject;
+		colObj1 = (btCollisionObject*)proxy1->m_clientObject;
+	}
+	else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
+	{
+		colObj0 = (btCollisionObject*)proxy1->m_clientObject;
+		colObj1 = (btCollisionObject*)proxy0->m_clientObject;
+	}
+	else
+	{
+		return true;
+	}
+	if (!colObj0 || !colObj1)
+		return false;
+	sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
+	objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
+	if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
+	{
+		return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
+	}
+	return true;
+}
 
 
 #ifdef NEW_BULLET_VEHICLE_SUPPORT
@@ -998,12 +1075,19 @@ PHY_IPhysicsController*	CcdPhysicsEnvironment::CreateSphereController(float radi
 {
 	
 	CcdConstructionInfo	cinfo;
+	// memory leak! The shape is not deleted by Bullet and we cannot add it to the KX_Scene.m_shapes list
 	cinfo.m_collisionShape = new btSphereShape(radius);
 	cinfo.m_MotionState = 0;
 	cinfo.m_physicsEnv = this;
-	cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE | btCollisionObject::CF_KINEMATIC_OBJECT;
+	// declare this object as Dyamic rather then static!!
+	// The reason as it is designed to detect all type of object, including static object
+	// It would cause static-static message to be printed on the console otherwise
+	cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE/* | btCollisionObject::CF_KINEMATIC_OBJECT*/;
 	DefaultMotionState* motionState = new DefaultMotionState();
 	cinfo.m_MotionState = motionState;
+	// we will add later the possibility to select the filter from option
+	cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
+	cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
 	motionState->m_worldTransform.setIdentity();
 	motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
 
@@ -1275,12 +1359,18 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
 {
 	CcdConstructionInfo	cinfo;
+	//This is a memory leak: Bullet does not delete the shape and it cannot be added to 
+	//the KX_Scene.m_shapes list -- too bad but that's not a lot of data
 	cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
 	cinfo.m_MotionState = 0;
 	cinfo.m_physicsEnv = this;
 	cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
 	DefaultMotionState* motionState = new DefaultMotionState();
 	cinfo.m_MotionState = motionState;
+	
+	// we will add later the possibility to select the filter from option
+	cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
+	cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
 	motionState->m_worldTransform.setIdentity();
 //	motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
 
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
index 66a6ed59c17..6d17c9af20a 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
@@ -42,6 +42,7 @@ class btBroadphaseInterface;
 class btOverlappingPairCache;
 class btIDebugDraw;
 class PHY_IVehicle;
+class CcdOverlapFilterCallBack;
 
 /// CcdPhysicsEnvironment is an experimental mainloop for physics simulation using optional continuous collision detection.
 /// Physics Environment takes care of stepping the simulation and is a container for physics entities.
@@ -49,9 +50,8 @@ class PHY_IVehicle;
 /// A derived class may be able to 'construct' entities by loading and/or converting
 class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 {
+	friend CcdOverlapFilterCallBack;
 	btVector3 m_gravity;
-	
-	
 
 protected:
 	btIDebugDraw*	m_debugDrawer;
@@ -166,7 +166,7 @@ protected:
 		virtual void addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user);
 		virtual void requestCollisionCallback(PHY_IPhysicsController* ctrl);
 		virtual void removeCollisionCallback(PHY_IPhysicsController* ctrl);
-
+		//These two methods are used *solely* to create controllers for Near/Radar sensor! Don't use for anything else
 		virtual PHY_IPhysicsController*	CreateSphereController(float radius,const PHY__Vector3& position);
 		virtual PHY_IPhysicsController* CreateConeController(float coneradius,float coneheight);
 	
@@ -229,10 +229,22 @@ protected:
 		
 		std::vector<WrapperVehicle*>	m_wrapperVehicles;
 
-		class	btDynamicsWorld*	m_dynamicsWorld;
+		//use explicit btDiscreteDynamicsWorld* so that we have access to 
+		//btDiscreteDynamicsWorld::addRigidBody(body,filter,group) 
+		//so that we can set the body collision filter/group at the time of creation 
+		//and not afterwards (breaks the collision system for radar/near sensor)
+		//Ideally we would like to have access to this function from the btDynamicsWorld interface
+		//class	btDynamicsWorld*	m_dynamicsWorld;
+		class	btDiscreteDynamicsWorld*	m_dynamicsWorld;
 		
 		class btConstraintSolver*	m_solver;
 
+		class btOverlappingPairCache* m_ownPairCache;
+
+		class CcdOverlapFilterCallBack* m_filterCallback;
+
+		class btDispatcher* m_ownDispatcher;
+
 		bool	m_scalingPropagated;
 
 		
diff --git a/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp b/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp
index 6cdf1d41dc1..67a74d11564 100644
--- a/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp
@@ -206,6 +206,8 @@ void SumoPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCa
 	case PHY_STATIC_RESPONSE:
 		sumoRespClass = PHY_STATIC_RESPONSE;
 		break;
+	case PHY_BROADPH_RESPONSE:
+		return;
 	default:
 		assert(0);
 		return;
diff --git a/source/gameengine/Physics/common/PHY_DynamicTypes.h b/source/gameengine/Physics/common/PHY_DynamicTypes.h
index 68b60192fbc..c289b9d8bcb 100644
--- a/source/gameengine/Physics/common/PHY_DynamicTypes.h
+++ b/source/gameengine/Physics/common/PHY_DynamicTypes.h
@@ -19,7 +19,6 @@ subject to the following restrictions:
 
 
 
-class	PHY_ResponseTable;
 
 class PHY_Shape;
 
@@ -40,10 +39,11 @@ struct	PHY__Vector3
 typedef enum
 {
 	PHY_FH_RESPONSE,
-	PHY_SENSOR_RESPONSE,		/* Touch Sensors */
+	PHY_SENSOR_RESPONSE,	/* Touch Sensors */
 	PHY_CAMERA_RESPONSE,	/* Visibility Culling */
 	PHY_OBJECT_RESPONSE,	/* Object Dynamic Geometry Response */
 	PHY_STATIC_RESPONSE,	/* Static Geometry Response */
+	PHY_BROADPH_RESPONSE,	/* broadphase Response */
 	
 	PHY_NUM_RESPONSE
 };
diff --git a/source/gameengine/Physics/common/PHY_IPhysicsController.h b/source/gameengine/Physics/common/PHY_IPhysicsController.h
index 931dc0e988c..ba1fb8473d3 100644
--- a/source/gameengine/Physics/common/PHY_IPhysicsController.h
+++ b/source/gameengine/Physics/common/PHY_IPhysicsController.h
@@ -85,7 +85,6 @@ class PHY_IPhysicsController
 
 		// dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
 		virtual	void		setRigidBody(bool rigid)=0;
-
 		// clientinfo for raycasts for example
 		virtual	void*				getNewClientInfo()=0;
 		virtual	void				setNewClientInfo(void* clientinfo)=0;
diff --git a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
index 5e4fd681914..d90b3d82e6b 100644
--- a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
+++ b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
@@ -107,6 +107,7 @@ class PHY_IPhysicsEnvironment
 		virtual void removeSensor(PHY_IPhysicsController* ctrl)=0;
 		virtual void addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)=0;
 		virtual void requestCollisionCallback(PHY_IPhysicsController* ctrl)=0;
+		//These two methods are *solely* used to create controllers for sensor! Don't use for anything else
 		virtual PHY_IPhysicsController*	CreateSphereController(float radius,const PHY__Vector3& position) =0;
 		virtual PHY_IPhysicsController* CreateConeController(float coneradius,float coneheight)=0;