BGE Scenegraph and View frustrum culling improvement.

This commit contains a number of performance improvements for the
BGE in the Scenegraph (parent relation between objects in the
scene) and view frustrum culling.

The scenegraph improvement consists in avoiding position update
if the object has not moved since last update and the removal
of redundant updates and synchronization with the physics engine.

The view frustrum culling improvement consists in using the DBVT
broadphase facility of Bullet to build a tree of graphical objects
in the scene. The elements of the tree are Aabb boxes (Aligned 
Axis Bounding Boxes) enclosing the objects. This provides good
precision in closed and opened scenes. This new culling system
is enabled by default but just in case, it can be disabled with
a button in the World settings. There is no do_version in this
commit but it will be added before the 2.49 release. For now you
must manually enable the DBVT culling option in World settings
when you open an old file.

The above improvements speed up scenegraph and culling up to 5x.
However, this performance improvement is only visible when
you have hundreds or thousands of objects.

The main interest of the DBVT tree is to allow easy occlusion
culling and automatic LOD system. This will be the object of further
improvements.

16 files changed, 254 insertions, 107 deletions

diff --git a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
index 062e9f7df50..435b2b5db19 100644
--- a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
+++ b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
@@ -417,13 +417,14 @@ void	KX_BulletPhysicsController::SetSumoTransform(bool nondynaonly)
 	{
 		if (!nondynaonly)
 		{
+			/*
 			btTransform worldTrans;
 			if (GetRigidBody())
 			{
 				GetRigidBody()->getMotionState()->getWorldTransform(worldTrans);
 				GetRigidBody()->setCenterOfMassTransform(worldTrans);
 			}
-			
+			*/
 			/*
 			scaling?
 			if (m_bDyna)
diff --git a/source/gameengine/Ketsji/KX_Camera.h b/source/gameengine/Ketsji/KX_Camera.h
index 6ed21506372..4accd4bc2f1 100644
--- a/source/gameengine/Ketsji/KX_Camera.h
+++ b/source/gameengine/Ketsji/KX_Camera.h
@@ -45,6 +45,7 @@ class KX_Camera : public KX_GameObject
 {
 	Py_Header;
 protected:
+	friend class KX_Scene;
 	/** Camera parameters (clips distances, focal lenght). These
 	 * params are closely tied to Blender. In the gameengine, only the
 	 * projection and modelview matrices are relevant. There's a
@@ -67,6 +68,7 @@ protected:
 	 * Storage for the projection matrix that is passed to the
 	 * rasterizer. */
 	MT_Matrix4x4 m_projection_matrix;
+	//MT_Matrix4x4 m_projection_matrix1;
 
 	/**
 	 * Storage for the modelview matrix that is passed to the
@@ -119,6 +121,16 @@ protected:
 	 * Extracts the bound sphere of the view frustum.
 	 */
 	void ExtractFrustumSphere();
+	/**
+	 * return the clip plane
+	 */
+	MT_Vector4 *GetNormalizedClipPlanes()
+	{
+		ExtractClipPlanes();
+		NormalizeClipPlanes();
+		return m_planes;
+	}
+
 public:
 
 	enum { INSIDE, INTERSECT, OUTSIDE } ;
diff --git a/source/gameengine/Ketsji/KX_GameObject.cpp b/source/gameengine/Ketsji/KX_GameObject.cpp
index 7f99a565769..339a955702a 100644
--- a/source/gameengine/Ketsji/KX_GameObject.cpp
+++ b/source/gameengine/Ketsji/KX_GameObject.cpp
@@ -55,6 +55,7 @@ typedef unsigned long uint_ptr;
 #include <stdio.h> // printf
 #include "SG_Controller.h"
 #include "KX_IPhysicsController.h"
+#include "PHY_IGraphicController.h"
 #include "SG_Node.h"
 #include "SG_Controller.h"
 #include "KX_ClientObjectInfo.h"
@@ -91,6 +92,7 @@ KX_GameObject::KX_GameObject(
 	m_bVisible(true),
 	m_bCulled(true),
 	m_pPhysicsController1(NULL),
+	m_pGraphicController(NULL),
 	m_pPhysicsEnvironment(NULL),
 	m_xray(false),
 	m_pHitObject(NULL),
@@ -132,6 +134,10 @@ KX_GameObject::~KX_GameObject()
 		}
 		m_pSGNode->SetSGClientObject(NULL);
 	}
+	if (m_pGraphicController)
+	{
+		delete m_pGraphicController;
+	}
 }
 
 
@@ -249,7 +255,7 @@ void KX_GameObject::SetParent(KX_Scene *scene, KX_GameObject* obj)
 		NodeSetLocalScale(scale1);
 		NodeSetLocalPosition(MT_Point3(newpos[0],newpos[1],newpos[2]));
 		NodeSetLocalOrientation(invori*NodeGetWorldOrientation());
-		NodeUpdateGS(0.f,true);
+		NodeUpdateGS(0.f);
 		// object will now be a child, it must be removed from the parent list
 		CListValue* rootlist = scene->GetRootParentList();
 		if (rootlist->RemoveValue(this))
@@ -269,6 +275,7 @@ void KX_GameObject::SetParent(KX_Scene *scene, KX_GameObject* obj)
 				rootobj->m_pPhysicsController1->AddCompoundChild(m_pPhysicsController1);
 			}
 		}
+		// graphically, the object hasn't change place, no need to update m_pGraphicController
 	}
 }
 
@@ -286,7 +293,7 @@ void KX_GameObject::RemoveParent(KX_Scene *scene)
 
 		// Remove us from our parent
 		GetSGNode()->DisconnectFromParent();
-		NodeUpdateGS(0.f,true);
+		NodeUpdateGS(0.f);
 		// the object is now a root object, add it to the parentlist
 		CListValue* rootlist = scene->GetRootParentList();
 		if (!rootlist->SearchValue(this))
@@ -303,12 +310,14 @@ void KX_GameObject::RemoveParent(KX_Scene *scene)
 			}
 			m_pPhysicsController1->RestoreDynamics();
 		}
+		// graphically, the object hasn't change place, no need to update m_pGraphicController
 	}
 }
 
 void KX_GameObject::ProcessReplica(KX_GameObject* replica)
 {
 	replica->m_pPhysicsController1 = NULL;
+	replica->m_pGraphicController = NULL;
 	replica->m_pSGNode = NULL;
 	replica->m_pClient_info = new KX_ClientObjectInfo(*m_pClient_info);
 	replica->m_pClient_info->m_gameobject = replica;
@@ -437,22 +446,18 @@ void KX_GameObject::RemoveMeshes()
 	m_meshes.clear();
 }
 
-
-
-void KX_GameObject::UpdateNonDynas()
+void KX_GameObject::UpdateTransform()
 {
 	if (m_pPhysicsController1)
-	{
+		// only update the transform of static object, dynamic object are handled differently
+		// note that for bullet, this does not even update the transform of static object
+		// but merely sets there collision flag to "kinematic" because the synchronization is 
+		// done differently during physics simulation
 		m_pPhysicsController1->SetSumoTransform(true);
-	}
-}
+	if (m_pGraphicController)
+		// update the culling tree
+		m_pGraphicController->SetGraphicTransform();
 
-
-
-void KX_GameObject::UpdateTransform()
-{
-	if (m_pPhysicsController1)
-		m_pPhysicsController1->SetSumoTransform(false);
 }
 
 void KX_GameObject::UpdateTransformFunc(SG_IObject* node, void* gameobj, void* scene)
@@ -832,6 +837,7 @@ void KX_GameObject::NodeSetLocalPosition(const MT_Point3& trans)
 	}
 
 	GetSGNode()->SetLocalPosition(trans);
+
 }
 
 
@@ -911,7 +917,7 @@ void KX_GameObject::NodeSetWorldPosition(const MT_Point3& trans)
 }
 
 
-void KX_GameObject::NodeUpdateGS(double time,bool bInitiator)
+void KX_GameObject::NodeUpdateGS(double time)
 {
 	if (GetSGNode())
 		GetSGNode()->UpdateWorldData(time);
@@ -1295,7 +1301,7 @@ int KX_GameObject::pyattr_set_position(void *self_v, const KX_PYATTRIBUTE_DEF *a
 		return 1;
 	
 	self->NodeSetLocalPosition(pos);
-	self->NodeUpdateGS(0.f,true);
+	self->NodeUpdateGS(0.f);
 	return 0;
 }
 
@@ -1319,10 +1325,10 @@ int KX_GameObject::pyattr_set_orientation(void *self_v, const KX_PYATTRIBUTE_DEF
 	if (PyMatTo(value, rot))
 	{
 		self->NodeSetLocalOrientation(rot);
-		self->NodeUpdateGS(0.f,true);
+		self->NodeUpdateGS(0.f);
 		return 0;
 	}
-	return 1;
+	PyErr_Clear();
 
 	if (PySequence_Size(value) == 4)
 	{
@@ -1331,7 +1337,7 @@ int KX_GameObject::pyattr_set_orientation(void *self_v, const KX_PYATTRIBUTE_DEF
 		{
 			rot.setRotation(qrot);
 			self->NodeSetLocalOrientation(rot);
-			self->NodeUpdateGS(0.f,true);
+			self->NodeUpdateGS(0.f);
 			return 0;
 		}
 		return 1;
@@ -1344,7 +1350,7 @@ int KX_GameObject::pyattr_set_orientation(void *self_v, const KX_PYATTRIBUTE_DEF
 		{
 			rot.setEuler(erot);
 			self->NodeSetLocalOrientation(rot);
-			self->NodeUpdateGS(0.f,true);
+			self->NodeUpdateGS(0.f);
 			return 0;
 		}
 		return 1;
@@ -1368,7 +1374,7 @@ int KX_GameObject::pyattr_set_scaling(void *self_v, const KX_PYATTRIBUTE_DEF *at
 		return 1;
 
 	self->NodeSetLocalScale(scale);
-	self->NodeUpdateGS(0.f,true);
+	self->NodeUpdateGS(0.f);
 	return 0;
 }
 
@@ -1929,7 +1935,7 @@ PyObject* KX_GameObject::PySetOrientation(PyObject* self, PyObject* value)
 	if (PyObject_IsMT_Matrix(value, 3) && PyMatTo(value, matrix))
 	{
 		NodeSetLocalOrientation(matrix);
-		NodeUpdateGS(0.f,true);
+		NodeUpdateGS(0.f);
 		Py_RETURN_NONE;
 	}
 
@@ -1938,7 +1944,7 @@ PyObject* KX_GameObject::PySetOrientation(PyObject* self, PyObject* value)
 	{
 		matrix.setRotation(quat);
 		NodeSetLocalOrientation(matrix);
-		NodeUpdateGS(0.f,true);
+		NodeUpdateGS(0.f);
 		Py_RETURN_NONE;
 	}
 	return NULL;
@@ -1959,7 +1965,7 @@ PyObject* KX_GameObject::PyAlignAxisToVect(PyObject* self, PyObject* args)
 			if (fac> 1.0) fac= 1.0;
 			
 			AlignAxisToVect(vect,axis,fac);
-			NodeUpdateGS(0.f,true);
+			NodeUpdateGS(0.f);
 			Py_RETURN_NONE;
 		}
 	}
@@ -1983,7 +1989,7 @@ PyObject* KX_GameObject::PySetPosition(PyObject* self, PyObject* value)
 	if (PyVecTo(value, pos))
 	{
 		NodeSetLocalPosition(pos);
-		NodeUpdateGS(0.f,true);
+		NodeUpdateGS(0.f);
 		Py_RETURN_NONE;
 	}
 
@@ -1996,7 +2002,7 @@ PyObject* KX_GameObject::PySetWorldPosition(PyObject* self, PyObject* value)
 	if (PyVecTo(value, pos))
 	{
 		NodeSetWorldPosition(pos);
-		NodeUpdateGS(0.f,true);
+		NodeUpdateGS(0.f);
 		Py_RETURN_NONE;
 	}
 
diff --git a/source/gameengine/Ketsji/KX_GameObject.h b/source/gameengine/Ketsji/KX_GameObject.h
index bc6b60102d6..9c7dda5e394 100644
--- a/source/gameengine/Ketsji/KX_GameObject.h
+++ b/source/gameengine/Ketsji/KX_GameObject.h
@@ -57,6 +57,7 @@ struct KX_ClientObjectInfo;
 class KX_RayCast;
 class RAS_MeshObject;
 class KX_IPhysicsController;
+class PHY_IGraphicController;
 class PHY_IPhysicsEnvironment;
 struct Object;
 
@@ -88,6 +89,7 @@ protected:
 	bool       							m_bCulled; 
 
 	KX_IPhysicsController*				m_pPhysicsController1;
+	PHY_IGraphicController*				m_pGraphicController;
 	// used for ray casting
 	PHY_IPhysicsEnvironment*			m_pPhysicsEnvironment;
 	STR_String							m_testPropName;
@@ -351,6 +353,19 @@ public:
 	}
 
 	/**
+	 * @return a pointer to the graphic controller owner by this class 
+	 */
+	PHY_IGraphicController* GetGraphicController()
+	{
+		return m_pGraphicController;
+	}
+
+	void SetGraphicController(PHY_IGraphicController* graphiccontroller) 
+	{ 
+		m_pGraphicController = graphiccontroller;
+	}
+
+	/**
 	 * @section Coordinate system manipulation functions
 	 */
 
@@ -367,8 +382,7 @@ public:
 
 		void						
 	NodeUpdateGS(
-		double time,
-		bool bInitiator
+		double time
 	);
 
 	const 
@@ -525,13 +539,6 @@ public:
 	static void UpdateTransformFunc(SG_IObject* node, void* gameobj, void* scene);
 
 	/**
-	 * Only update the transform if it's a non-dynamic object
-	 */
-		void 
-	UpdateNonDynas(
-	);
-
-	/**
 	 * Function to set IPO option at start of IPO
 	 */ 
 		void	
diff --git a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
index 97b4213b8bd..70ae0e4b937 100644
--- a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
+++ b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
@@ -412,7 +412,7 @@ else
 
 
 	// Compute the number of logic frames to do each update (fixed tic bricks)
-	int frames =int(deltatime*m_ticrate);
+	int frames =int(deltatime*m_ticrate+1e-6);
 //	if (frames>1)
 //		printf("****************************************");
 //	printf("dt = %f, deltatime = %f, frames = %d\n",dt, deltatime,frames);
@@ -465,12 +465,15 @@ else
 
 				
 				m_logger->StartLog(tc_network, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_NETWORK);
 				scene->GetNetworkScene()->proceed(m_frameTime);
 	
-				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
-				scene->UpdateParents(m_frameTime);
+				//m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+				//SG_SetActiveStage(SG_STAGE_NETWORK_UPDATE);
+				//scene->UpdateParents(m_frameTime);
 				
 				m_logger->StartLog(tc_physics, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_PHYSICS1);
 				// set Python hooks for each scene
 				PHY_SetActiveEnvironment(scene->GetPhysicsEnvironment());
 				KX_SetActiveScene(scene);
@@ -479,31 +482,37 @@ else
 				
 				// Update scenegraph after physics step. This maps physics calculations
 				// into node positions.		
-				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
-				scene->UpdateParents(m_frameTime);
+				//m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+				//SG_SetActiveStage(SG_STAGE_PHYSICS1_UPDATE);
+				//scene->UpdateParents(m_frameTime);
 				
 				// Process sensors, and controllers
 				m_logger->StartLog(tc_logic, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_CONTROLLER);
 				scene->LogicBeginFrame(m_frameTime);
 	
 				// Scenegraph needs to be updated again, because Logic Controllers 
 				// can affect the local matrices.
 				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_CONTROLLER_UPDATE);
 				scene->UpdateParents(m_frameTime);
 	
 				// Process actuators
 	
 				// Do some cleanup work for this logic frame
 				m_logger->StartLog(tc_logic, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_ACTUATOR);
 				scene->LogicUpdateFrame(m_frameTime, true);
 				
 				scene->LogicEndFrame();
 	
 				// Actuators can affect the scenegraph
 				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_ACTUATOR_UPDATE);
 				scene->UpdateParents(m_frameTime);
 				
 				m_logger->StartLog(tc_physics, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_PHYSICS2);
 				scene->GetPhysicsEnvironment()->beginFrame();
 		
 				// Perform physics calculations on the scene. This can involve 
@@ -511,6 +520,7 @@ else
 				scene->GetPhysicsEnvironment()->proceedDeltaTime(m_frameTime,1.0/m_ticrate);//m_deltatimerealDeltaTime);
 
 				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_PHYSICS2_UPDATE);
 				scene->UpdateParents(m_frameTime);
 			
 			
@@ -574,6 +584,7 @@ else
 				KX_SetActiveScene(scene);
 				
 				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_PHYSICS1);
 				scene->UpdateParents(m_clockTime);
 
 				// Perform physics calculations on the scene. This can involve 
@@ -583,6 +594,7 @@ else
 				// Update scenegraph after physics step. This maps physics calculations
 				// into node positions.		
 				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_PHYSICS2);
 				scene->UpdateParents(m_clockTime);
 				
 				// Do some cleanup work for this logic frame
@@ -591,6 +603,7 @@ else
 
 				// Actuators can affect the scenegraph
 				m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+				SG_SetActiveStage(SG_STAGE_ACTUATOR);
 				scene->UpdateParents(m_clockTime);
 				 
  				scene->setSuspendedTime(0.0);
@@ -622,6 +635,7 @@ void KX_KetsjiEngine::Render()
 	const RAS_FrameSettings &framesettings = firstscene->GetFramingType();
 
 	m_logger->StartLog(tc_rasterizer, m_kxsystem->GetTimeInSeconds(), true);
+	SG_SetActiveStage(SG_STAGE_RENDER);
 
 	// hiding mouse cursor each frame
 	// (came back when going out of focus and then back in again)
@@ -1102,14 +1116,22 @@ void KX_KetsjiEngine::RenderFrame(KX_Scene* scene, KX_Camera* cam)
 		cam->GetCameraLocation(), cam->GetCameraOrientation());
 	cam->SetModelviewMatrix(viewmat);
 
-	scene->UpdateMeshTransformations();
+	//redundant, already done in 
+	//scene->UpdateMeshTransformations();
 
 	// The following actually reschedules all vertices to be
 	// redrawn. There is a cache between the actual rescheduling
 	// and this call though. Visibility is imparted when this call
 	// runs through the individual objects.
+
+	m_logger->StartLog(tc_scenegraph, m_kxsystem->GetTimeInSeconds(), true);
+	SG_SetActiveStage(SG_STAGE_CULLING);
+
 	scene->CalculateVisibleMeshes(m_rasterizer,cam);
 
+	m_logger->StartLog(tc_rasterizer, m_kxsystem->GetTimeInSeconds(), true);
+	SG_SetActiveStage(SG_STAGE_RENDER);
+
 	scene->RenderBuckets(camtrans, m_rasterizer, m_rendertools);
 	
 	if (scene->GetPhysicsEnvironment())
@@ -1166,15 +1188,17 @@ void KX_KetsjiEngine::AddScene(KX_Scene* scene)
 void KX_KetsjiEngine::PostProcessScene(KX_Scene* scene)
 {
 	bool override_camera = (m_overrideCam && (scene->GetName() == m_overrideSceneName));
-	
-		// if there is no activecamera, or the camera is being
-		// overridden we need to construct a temporarily camera
+
+	SG_SetActiveStage(SG_STAGE_SCENE);
+
+	// if there is no activecamera, or the camera is being
+	// overridden we need to construct a temporarily camera
 	if (!scene->GetActiveCamera() || override_camera)
 	{
 		KX_Camera* activecam = NULL;
 
 		RAS_CameraData camdata = RAS_CameraData();
-		activecam = new KX_Camera(scene,KX_Scene::m_callbacks,camdata, false);
+		activecam = new KX_Camera(scene,KX_Scene::m_callbacks,camdata);
 		activecam->SetName("__default__cam__");
 	
 			// set transformation
@@ -1186,11 +1210,11 @@ void KX_KetsjiEngine::PostProcessScene(KX_Scene* scene)
 			
 			activecam->NodeSetLocalPosition(camtrans.getOrigin());
 			activecam->NodeSetLocalOrientation(camtrans.getBasis());
-			activecam->NodeUpdateGS(0,true);
+			activecam->NodeUpdateGS(0);
 		} else {
 			activecam->NodeSetLocalPosition(MT_Point3(0.0, 0.0, 0.0));
 			activecam->NodeSetLocalOrientation(MT_Vector3(0.0, 0.0, 0.0));
-			activecam->NodeUpdateGS(0,true);
+			activecam->NodeUpdateGS(0);
 		}
 
 		scene->AddCamera(activecam);
diff --git a/source/gameengine/Ketsji/KX_Light.cpp b/source/gameengine/Ketsji/KX_Light.cpp
index 7b5b77ccacf..f996f86d751 100644
--- a/source/gameengine/Ketsji/KX_Light.cpp
+++ b/source/gameengine/Ketsji/KX_Light.cpp
@@ -159,7 +159,7 @@ void KX_LightObject::BindShadowBuffer(RAS_IRasterizer *ras, KX_Camera *cam, MT_T
 	
 	cam->NodeSetLocalPosition(camtrans.getOrigin());
 	cam->NodeSetLocalOrientation(camtrans.getBasis());
-	cam->NodeUpdateGS(0,true);
+	cam->NodeUpdateGS(0);
 
 	/* setup rasterizer transformations */
 	ras->SetProjectionMatrix(projectionmat);
diff --git a/source/gameengine/Ketsji/KX_MotionState.cpp b/source/gameengine/Ketsji/KX_MotionState.cpp
index 15f100af915..00d9a32eb38 100644
--- a/source/gameengine/Ketsji/KX_MotionState.cpp
+++ b/source/gameengine/Ketsji/KX_MotionState.cpp
@@ -44,7 +44,7 @@ KX_MotionState::~KX_MotionState()
 
 void	KX_MotionState::getWorldPosition(float& posX,float& posY,float& posZ)
 {
-	MT_Point3 pos = m_node->GetWorldPosition();
+	const MT_Point3& pos = m_node->GetWorldPosition();
 	posX = pos[0];
 	posY = pos[1];
 	posZ = pos[2];
@@ -52,7 +52,7 @@ void	KX_MotionState::getWorldPosition(float& posX,float& posY,float& posZ)
 
 void	KX_MotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
 {
-	MT_Vector3 scale = m_node->GetWorldScaling();
+	const MT_Vector3& scale = m_node->GetWorldScaling();
 	scaleX = scale[0];
 	scaleY = scale[1];
 	scaleZ = scale[2];
@@ -60,17 +60,23 @@ void	KX_MotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
 
 void	KX_MotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
 {
-	MT_Quaternion orn = m_node->GetWorldOrientation().getRotation();
+	MT_Quaternion& orn = m_node->GetWorldOrientation().getRotation();
 	quatIma0 = orn[0];
 	quatIma1 = orn[1];
 	quatIma2 = orn[2];
 	quatReal = orn[3];
 }
 	
+void	KX_MotionState::getWorldOrientation(float* ori)
+{
+	const MT_Matrix3x3& mat = m_node->GetWorldOrientation();
+	mat.getValue(ori);
+}
+	
 void	KX_MotionState::setWorldPosition(float posX,float posY,float posZ)
 {
 	m_node->SetLocalPosition(MT_Point3(posX,posY,posZ));
-	m_node->SetWorldPosition(MT_Point3(posX,posY,posZ));
+	//m_node->SetWorldPosition(MT_Point3(posX,posY,posZ));
 }
 
 void	KX_MotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
@@ -82,13 +88,15 @@ void	KX_MotionState::setWorldOrientation(float quatIma0,float quatIma1,float qua
 	orn[3] = quatReal;
 
 	m_node->SetLocalOrientation(orn);
-	m_node->SetWorldOrientation(orn);
+	//m_node->SetWorldOrientation(orn);
 
 }
 
 void	KX_MotionState::calculateWorldTransformations()
 {
-	m_node->ComputeWorldTransforms(NULL);
+	//Not needed, will be done in KX_Scene::UpdateParents() after the physics simulation
+	//bool parentUpdated = false;
+	//m_node->ComputeWorldTransforms(NULL, parentUpdated);
 }
 
  
diff --git a/source/gameengine/Ketsji/KX_MotionState.h b/source/gameengine/Ketsji/KX_MotionState.h
index c83af664817..7ba3ca2f85c 100644
--- a/source/gameengine/Ketsji/KX_MotionState.h
+++ b/source/gameengine/Ketsji/KX_MotionState.h
@@ -44,6 +44,7 @@ public:
 	virtual void	getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal);
 	virtual void	setWorldPosition(float posX,float posY,float posZ);
 	virtual	void	setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal);
+	virtual void	getWorldOrientation(float* ori);
 
 	virtual	void	calculateWorldTransformations();
 };
diff --git a/source/gameengine/Ketsji/KX_NearSensor.cpp b/source/gameengine/Ketsji/KX_NearSensor.cpp
index cd753210184..b9d1939e5db 100644
--- a/source/gameengine/Ketsji/KX_NearSensor.cpp
+++ b/source/gameengine/Ketsji/KX_NearSensor.cpp
@@ -127,13 +127,10 @@ CValue* KX_NearSensor::GetReplica()
 		}
 		
 	}
-	//static_cast<KX_TouchEventManager*>(m_eventmgr)->RegisterSensor(this);
-	//todo: make sure replication works fine
-	//>m_sumoObj = new SM_Object(DT_NewSphere(0.0),NULL,NULL,NULL);
-	//replica->m_sumoObj->setMargin(m_Margin);
-	//replica->m_sumoObj->setClientObject(replica->m_client_info);
-	
-	((KX_GameObject*)replica->GetParent())->GetSGNode()->ComputeWorldTransforms(NULL);
+	//Wrong: the parent object could be a child, this code works only if it is a root parent.
+	//Anyway, at this stage, the parent object is already synchronized, nothing to do.
+	//bool parentUpdated = false;
+	//((KX_GameObject*)replica->GetParent())->GetSGNode()->ComputeWorldTransforms(NULL, parentUpdated);
 	replica->SynchronizeTransform();
 	
 	return replica;
@@ -154,8 +151,10 @@ void KX_NearSensor::ReParent(SCA_IObject* parent)
 	client_info->m_sensors.push_back(this);
 	SCA_ISensor::ReParent(parent);
 */
-	((KX_GameObject*)GetParent())->GetSGNode()->ComputeWorldTransforms(NULL);
-	SynchronizeTransform();
+	//Not needed, was done in GetReplica() already
+	//bool parentUpdated = false;
+	//((KX_GameObject*)GetParent())->GetSGNode()->ComputeWorldTransforms(NULL,parentUpdated);
+	//SynchronizeTransform();
 	SCA_ISensor::ReParent(parent);
 }
 
diff --git a/source/gameengine/Ketsji/KX_RadarSensor.cpp b/source/gameengine/Ketsji/KX_RadarSensor.cpp
index 355ac89a926..b9abe69633e 100644
--- a/source/gameengine/Ketsji/KX_RadarSensor.cpp
+++ b/source/gameengine/Ketsji/KX_RadarSensor.cpp
@@ -100,8 +100,9 @@ CValue* KX_RadarSensor::GetReplica()
 	//>m_sumoObj = new SM_Object(DT_NewCone(m_coneradius, m_coneheight),NULL,NULL,NULL);
 	//replica->m_sumoObj->setMargin(m_Margin);
 	//replica->m_sumoObj->setClientObject(replica->m_client_info);
-	
-	((KX_GameObject*)replica->GetParent())->GetSGNode()->ComputeWorldTransforms(NULL);
+	//Wrong: see KX_TouchSensor
+	//bool parentUpdated = false;
+	//((KX_GameObject*)replica->GetParent())->GetSGNode()->ComputeWorldTransforms(NULL,parentUpdated);
 	replica->SynchronizeTransform();
 	
 	return replica;
diff --git a/source/gameengine/Ketsji/KX_SG_BoneParentNodeRelationship.cpp b/source/gameengine/Ketsji/KX_SG_BoneParentNodeRelationship.cpp
index 151270cbd68..0e7571031e8 100644
--- a/source/gameengine/Ketsji/KX_SG_BoneParentNodeRelationship.cpp
+++ b/source/gameengine/Ketsji/KX_SG_BoneParentNodeRelationship.cpp
@@ -57,7 +57,8 @@ New(Bone* bone
 KX_BoneParentRelation::
 UpdateChildCoordinates(
 	SG_Spatial * child,
-	const SG_Spatial * parent
+	const SG_Spatial * parent,
+	bool& parentUpdated	
 ){
 	MT_assert(child != NULL);
 	
@@ -67,6 +68,8 @@ UpdateChildCoordinates(
 	const MT_Vector3 & child_scale = child->GetLocalScale();
 	const MT_Point3 & child_pos = child->GetLocalPosition();
 	const MT_Matrix3x3 & child_rotation = child->GetLocalOrientation();
+	// we don't know if the armature has been updated or not, assume yes
+	parentUpdated = true;
 
 	// the childs world locations which we will update.	
 	
@@ -122,7 +125,7 @@ UpdateChildCoordinates(
 	else {
 		child->SetWorldFromLocalTransform();
 	}
-	
+	child->SetModified(false);
 	return valid_parent_transform;
 }
 
diff --git a/source/gameengine/Ketsji/KX_SG_BoneParentNodeRelationship.h b/source/gameengine/Ketsji/KX_SG_BoneParentNodeRelationship.h
index 2a19d8a1784..c9baf228855 100644
--- a/source/gameengine/Ketsji/KX_SG_BoneParentNodeRelationship.h
+++ b/source/gameengine/Ketsji/KX_SG_BoneParentNodeRelationship.h
@@ -82,7 +82,8 @@ public :
 		bool
 	UpdateChildCoordinates(
 		SG_Spatial * child,
-		const SG_Spatial * parent
+		const SG_Spatial * parent,
+		bool& parentUpdated
 	);
 
 	/**
diff --git a/source/gameengine/Ketsji/KX_SG_NodeRelationships.cpp b/source/gameengine/Ketsji/KX_SG_NodeRelationships.cpp
index 0729ec8a902..87ff3b53911 100644
--- a/source/gameengine/Ketsji/KX_SG_NodeRelationships.cpp
+++ b/source/gameengine/Ketsji/KX_SG_NodeRelationships.cpp
@@ -51,13 +51,20 @@ New(
 KX_NormalParentRelation::
 UpdateChildCoordinates(
 	SG_Spatial * child,
-	const SG_Spatial * parent
+	const SG_Spatial * parent,
+	bool& parentUpdated	
 ){
 	MT_assert(child != NULL);
 
+	if (!parentUpdated && !child->IsModified())
+		return false;
+
+	parentUpdated = true;
+
 	if (parent==NULL) { /* Simple case */
 		child->SetWorldFromLocalTransform();
-		return false;
+		child->SetModified(false);
+		return true; //false;
 	}
 	else {
 		// the childs world locations which we will update.	
@@ -68,6 +75,7 @@ UpdateChildCoordinates(
 		child->SetWorldScale(p_world_scale * child->GetLocalScale());
 		child->SetWorldOrientation(p_world_rotation * child->GetLocalOrientation());
 		child->SetWorldPosition(p_world_pos + p_world_scale * (p_world_rotation * child->GetLocalPosition()));
+		child->SetModified(false);
 		return true;
 	}
 }
@@ -112,10 +120,15 @@ New(
 KX_VertexParentRelation::
 UpdateChildCoordinates(
 	SG_Spatial * child,
-	const SG_Spatial * parent
+	const SG_Spatial * parent,
+	bool& parentUpdated	
 ){
 
 	MT_assert(child != NULL);
+
+	if (!parentUpdated && !child->IsModified())
+		return false;
+
 	child->SetWorldScale(child->GetLocalScale());
 	
 	if (parent)
@@ -124,7 +137,8 @@ UpdateChildCoordinates(
 		child->SetWorldPosition(child->GetLocalPosition());
 	
 	child->SetWorldOrientation(child->GetLocalOrientation());
-	return parent != NULL;
+	child->SetModified(false);
+	return true; //parent != NULL;
 }
 
 /** 
@@ -172,10 +186,14 @@ New(
 KX_SlowParentRelation::
 UpdateChildCoordinates(
 	SG_Spatial * child,
-	const SG_Spatial * parent
+	const SG_Spatial * parent,
+	bool& parentUpdated	
 ){
 	MT_assert(child != NULL);
 
+	// the child will move even if the parent is not
+	parentUpdated = true;
+
 	const MT_Vector3 & child_scale = child->GetLocalScale();
 	const MT_Point3 & child_pos = child->GetLocalPosition();
 	const MT_Matrix3x3 & child_rotation = child->GetLocalOrientation();
@@ -252,8 +270,9 @@ UpdateChildCoordinates(
 	child->SetWorldScale(child_w_scale);
 	child->SetWorldPosition(child_w_pos);
 	child->SetWorldOrientation(child_w_rotation);
+	child->SetModified(false);
 	
-	return parent != NULL;
+	return true; //parent != NULL;
 }
 
 /** 
diff --git a/source/gameengine/Ketsji/KX_SG_NodeRelationships.h b/source/gameengine/Ketsji/KX_SG_NodeRelationships.h
index faa650106c8..d8fb9211f21 100644
--- a/source/gameengine/Ketsji/KX_SG_NodeRelationships.h
+++ b/source/gameengine/Ketsji/KX_SG_NodeRelationships.h
@@ -71,7 +71,8 @@ public :
 		bool
 	UpdateChildCoordinates(
 		SG_Spatial * child,
-		const SG_Spatial * parent
+		const SG_Spatial * parent,
+		bool& parentUpdated	
 	);
 
 	/** 
@@ -115,7 +116,8 @@ public :
 		bool
 	UpdateChildCoordinates(
 		SG_Spatial * child,
-		const SG_Spatial * parent
+		const SG_Spatial * parent,
+		bool& parentUpdated	
 	);
 
 	/** 
@@ -166,7 +168,8 @@ public :
 		bool
 	UpdateChildCoordinates(
 		SG_Spatial * child,
-		const SG_Spatial * parent
+		const SG_Spatial * parent,
+		bool& parentUpdated	
 	);
 
 	/** 
diff --git a/source/gameengine/Ketsji/KX_Scene.cpp b/source/gameengine/Ketsji/KX_Scene.cpp
index 04d6bd75f92..7eed1cc387b 100644
--- a/source/gameengine/Ketsji/KX_Scene.cpp
+++ b/source/gameengine/Ketsji/KX_Scene.cpp
@@ -76,7 +76,9 @@
 #include "NG_NetworkScene.h"
 #include "PHY_IPhysicsEnvironment.h"
 #include "KX_IPhysicsController.h"
+#include "PHY_IGraphicController.h"
 #include "KX_BlenderSceneConverter.h"
+#include "KX_MotionState.h"
 
 #include "BL_ShapeDeformer.h"
 #include "BL_DeformableGameObject.h"
@@ -133,6 +135,7 @@ KX_Scene::KX_Scene(class SCA_IInputDevice* keyboarddevice,
 	m_suspendedtime = 0.0;
 	m_suspendeddelta = 0.0;
 
+	m_dbvt_culling = false;
 	m_activity_culling = false;
 	m_suspend = false;
 	m_isclearingZbuffer = true;
@@ -407,6 +410,13 @@ void KX_Scene::RemoveNodeDestructObject(class SG_IObject* node,class CValue* gam
 		// will in any case be deleted. This ensures that the object will not try to use the node
 		// when it is finally deleted (see KX_GameObject destructor)
 		orgobj->SetSGNode(NULL);
+		PHY_IGraphicController* ctrl = orgobj->GetGraphicController();
+		if (ctrl)
+		{
+			// a graphic controller is set, we must delete it as the node will be deleted
+			delete ctrl;
+			orgobj->SetGraphicController(NULL);
+		}
 	}
 	if (node)
 		delete node;
@@ -485,7 +495,14 @@ KX_GameObject* KX_Scene::AddNodeReplicaObject(class SG_IObject* node, class CVal
 			replicanode->AddSGController(replicacontroller);
 		}
 	}
-	
+	// replicate graphic controller
+	if (orgobj->GetGraphicController())
+	{
+		PHY_IMotionState* motionstate = new KX_MotionState(newobj->GetSGNode());
+		PHY_IGraphicController* newctrl = orgobj->GetGraphicController()->GetReplica(motionstate);
+		newctrl->setNewClientInfo(newobj->getClientInfo());
+		newobj->SetGraphicController(newctrl);
+	}
 	return newobj;
 }
 
@@ -792,6 +809,24 @@ SCA_IObject* KX_Scene::AddReplicaObject(class CValue* originalobject,
 			replica->GetSGNode()->AddChild(childreplicanode);
 	}
 
+	// At this stage all the objects in the hierarchy have been duplicated,
+	// we can update the scenegraph, we need it for the duplication of logic
+	MT_Point3 newpos = ((KX_GameObject*) parentobject)->NodeGetWorldPosition();
+	replica->NodeSetLocalPosition(newpos);
+
+	MT_Matrix3x3 newori = ((KX_GameObject*) parentobject)->NodeGetWorldOrientation();
+	replica->NodeSetLocalOrientation(newori);
+	
+	// get the rootnode's scale
+	MT_Vector3 newscale = parentobj->GetSGNode()->GetRootSGParent()->GetLocalScale();
+
+	// set the replica's relative scale with the rootnode's scale
+	replica->NodeSetRelativeScale(newscale);
+
+	replica->GetSGNode()->UpdateWorldData(0);
+	replica->GetSGNode()->SetBBox(originalobj->GetSGNode()->BBox());
+	replica->GetSGNode()->SetRadius(originalobj->GetSGNode()->Radius());
+
 	// now replicate logic
 	vector<KX_GameObject*>::iterator git;
 	for (git = m_logicHierarchicalGameObjects.begin();!(git==m_logicHierarchicalGameObjects.end());++git)
@@ -814,21 +849,6 @@ SCA_IObject* KX_Scene::AddReplicaObject(class CValue* originalobject,
 		ReplicateLogic((*git));
 	}
 	
-	MT_Point3 newpos = ((KX_GameObject*) parentobject)->NodeGetWorldPosition();
-	replica->NodeSetLocalPosition(newpos);
-
-	MT_Matrix3x3 newori = ((KX_GameObject*) parentobject)->NodeGetWorldOrientation();
-	replica->NodeSetLocalOrientation(newori);
-	
-	// get the rootnode's scale
-	MT_Vector3 newscale = parentobj->GetSGNode()->GetRootSGParent()->GetLocalScale();
-
-	// set the replica's relative scale with the rootnode's scale
-	replica->NodeSetRelativeScale(newscale);
-
-	replica->GetSGNode()->UpdateWorldData(0);
-	replica->GetSGNode()->SetBBox(originalobj->GetSGNode()->BBox());
-	replica->GetSGNode()->SetRadius(originalobj->GetSGNode()->Radius());
 	// check if there are objects with dupligroup in the hierarchy
 	vector<KX_GameObject*> duplilist;
 	for (git = m_logicHierarchicalGameObjects.begin();!(git==m_logicHierarchicalGameObjects.end());++git)
@@ -1163,7 +1183,6 @@ void KX_Scene::UpdateMeshTransformations()
 	{
 		KX_GameObject* gameobj = (KX_GameObject*)m_objectlist->GetValue(i);
 		gameobj->GetOpenGLMatrix();
-//		gameobj->UpdateNonDynas();
 	}
 }
 
@@ -1298,21 +1317,46 @@ void KX_Scene::MarkVisible(RAS_IRasterizer* rasty, KX_GameObject* gameobj,KX_Cam
 	}
 }
 
+void KX_Scene::PhysicsCullingCallback(KX_ClientObjectInfo* objectInfo, void* cullingInfo)
+{
+	KX_GameObject* gameobj = objectInfo->m_gameobject;
+	if (!gameobj->GetVisible())
+		// ideally, invisible objects should be removed from the culling tree temporarily
+		return;
+	if(((CullingInfo*)cullingInfo)->m_layer && !(gameobj->GetLayer() & ((CullingInfo*)cullingInfo)->m_layer))
+		// used for shadow: object is not in shadow layer
+		return;
+
+	// make object visible
+	gameobj->SetCulled(false);
+	gameobj->UpdateBuckets(false);
+}
+
 void KX_Scene::CalculateVisibleMeshes(RAS_IRasterizer* rasty,KX_Camera* cam, int layer)
 {
-// FIXME: When tree is operational
-#if 1
-	// do this incrementally in the future
-	for (int i = 0; i < m_objectlist->GetCount(); i++)
+	bool dbvt_culling = false;
+	if (m_dbvt_culling) 
 	{
-		MarkVisible(rasty, static_cast<KX_GameObject*>(m_objectlist->GetValue(i)), cam, layer);
+		// test culling through Bullet
+		PHY__Vector4 planes[5];
+		// get the clip planes
+		MT_Vector4* cplanes = cam->GetNormalizedClipPlanes();
+		// and convert
+		planes[0].setValue(cplanes[0].getValue());
+		planes[1].setValue(cplanes[1].getValue());
+		planes[2].setValue(cplanes[2].getValue());
+		planes[3].setValue(cplanes[3].getValue());
+		planes[4].setValue(cplanes[5].getValue());
+		CullingInfo info(layer);
+		dbvt_culling = m_physicsEnvironment->cullingTest(PhysicsCullingCallback,&info,planes,5);
+	}
+	if (!dbvt_culling) {
+		// the physics engine couldn't help us, do it the hard way
+		for (int i = 0; i < m_objectlist->GetCount(); i++)
+		{
+			MarkVisible(rasty, static_cast<KX_GameObject*>(m_objectlist->GetValue(i)), cam, layer);
+		}
 	}
-#else
-	if (cam->GetFrustumCulling())
-		MarkVisible(m_objecttree, rasty, cam, layer);
-	else
-		MarkSubTreeVisible(m_objecttree, rasty, true, cam, layer);
-#endif
 }
 
 // logic stuff
@@ -1393,7 +1437,7 @@ void KX_Scene::UpdateParents(double curtime)
 	for (int i=0; i<GetRootParentList()->GetCount(); i++)
 	{
 		KX_GameObject* parentobj = (KX_GameObject*)GetRootParentList()->GetValue(i);
-		parentobj->NodeUpdateGS(curtime,true);
+		parentobj->NodeUpdateGS(curtime);
 	}
 }
 
@@ -1588,6 +1632,7 @@ PyAttributeDef KX_Scene::Attributes[] = {
 	KX_PYATTRIBUTE_BOOL_RO("suspended",			KX_Scene, m_suspend),
 	KX_PYATTRIBUTE_BOOL_RO("activity_culling",	KX_Scene, m_activity_culling),
 	KX_PYATTRIBUTE_FLOAT_RW("activity_culling_radius", 0.5f, FLT_MAX, KX_Scene, m_activity_box_radius),
+	KX_PYATTRIBUTE_BOOL_RO("dbvt_culling",		KX_Scene, m_dbvt_culling),
 	KX_PYATTRIBUTE_RO_FUNCTION("__dict__",		KX_Scene, pyattr_get_dir_dict),
 	{ NULL }	//Sentinel
 };
diff --git a/source/gameengine/Ketsji/KX_Scene.h b/source/gameengine/Ketsji/KX_Scene.h
index df51fcec8f7..9f05ddf70c2 100644
--- a/source/gameengine/Ketsji/KX_Scene.h
+++ b/source/gameengine/Ketsji/KX_Scene.h
@@ -85,6 +85,8 @@ class RAS_IRenderTools;
 class SCA_JoystickManager;
 class btCollisionShape;
 class KX_BlenderSceneConverter;
+struct KX_ClientObjectInfo;
+
 /**
  * The KX_Scene holds all data for an independent scene. It relates
  * KX_Objects to the specific objects in the modules.
@@ -92,6 +94,12 @@ class KX_BlenderSceneConverter;
 class KX_Scene : public PyObjectPlus, public SCA_IScene
 {
 	Py_Header;
+
+	struct CullingInfo {
+		int m_layer;
+		CullingInfo(int layer) : m_layer(layer) {}
+	};
+
 protected:
 	RAS_BucketManager*	m_bucketmanager;
 	CListValue*			m_tempObjectList;
@@ -252,6 +260,11 @@ protected:
 	bool m_activity_culling;
 	
 	/**
+	 * Toggle to enable or disable culling via DBVT broadphase of Bullet.
+	 */
+	bool m_dbvt_culling;
+	
+	/**
 	 * The framing settings used by this scene
 	 */
 
@@ -269,6 +282,7 @@ protected:
 	void MarkVisible(SG_Tree *node, RAS_IRasterizer* rasty, KX_Camera*cam,int layer=0);
 	void MarkSubTreeVisible(SG_Tree *node, RAS_IRasterizer* rasty, bool visible, KX_Camera*cam,int layer=0);
 	void MarkVisible(RAS_IRasterizer* rasty, KX_GameObject* gameobj, KX_Camera*cam, int layer=0);
+	static void PhysicsCullingCallback(KX_ClientObjectInfo* objectInfo, void* cullingInfo);
 
 	double				m_suspendedtime;
 	double				m_suspendeddelta;
@@ -530,6 +544,9 @@ public:
 	bool IsSuspended();
 	bool IsClearingZBuffer();
 	void EnableZBufferClearing(bool isclearingZbuffer);
+	// use of DBVT tree for camera culling
+	void SetDbvtCameraCulling(bool b) { m_dbvt_culling = b; };
+	bool GetDbvtCameraCulling() { return m_dbvt_culling; };
 	
 	void SetSceneConverter(class KX_BlenderSceneConverter* sceneConverter);