Added resolveCombinedVelocities()

Fixed drot actuator.  The rotation matrix was being mutilated by passing a float[9] instead of float[12].

15 files changed, 82 insertions, 33 deletions

diff --git a/source/gameengine/Ketsji/KX_GameObject.cpp b/source/gameengine/Ketsji/KX_GameObject.cpp
index 2f30ac645a0..f49090acc0d 100644
--- a/source/gameengine/Ketsji/KX_GameObject.cpp
+++ b/source/gameengine/Ketsji/KX_GameObject.cpp
@@ -237,12 +237,10 @@ void KX_GameObject::ApplyRotation(const MT_Vector3& drot,bool local)
 	MT_Matrix3x3 rotmat(drot);
 	rotmat.transpose();
 	
-	//if (m_pPhysicsController1) // (IsDynamic())
-	//	m_pPhysicsController1->RelativeRotate(rotmat_,local); 
+	if (m_pPhysicsController1) // (IsDynamic())
+		m_pPhysicsController1->RelativeRotate(rotmat,local); 
 	// in worldspace
 	GetSGNode()->RelativeRotate(rotmat,local);
-	if (m_pPhysicsController1)
-		m_pPhysicsController1->setOrientation(NodeGetWorldOrientation().getRotation());
 }
 
 
@@ -459,6 +457,20 @@ void KX_GameObject::setAngularVelocity(const MT_Vector3& ang_vel,bool local)
 		m_pPhysicsController1->SetAngularVelocity(ang_vel,local);
 }
 
+void KX_GameObject::ResolveCombinedVelocities(
+	const MT_Vector3 & lin_vel,
+	const MT_Vector3 & ang_vel,
+	bool lin_vel_local,
+	bool ang_vel_local
+){
+	if (m_pPhysicsController1)
+	{
+		m_pPhysicsController1->resolveCombinedVelocities(
+			lin_vel_local ? NodeGetWorldOrientation() * lin_vel : lin_vel,
+			ang_vel_local ? NodeGetWorldOrientation() * ang_vel : ang_vel			
+		);		
+	}
+}
 
 
 void KX_GameObject::SetObjectColor(const MT_Vector4& rgbavec)
diff --git a/source/gameengine/Ketsji/KX_GameObject.h b/source/gameengine/Ketsji/KX_GameObject.h
index 6b3101be34d..9fb11f1f31d 100644
--- a/source/gameengine/Ketsji/KX_GameObject.h
+++ b/source/gameengine/Ketsji/KX_GameObject.h
@@ -247,6 +247,13 @@ public:
 	);
 
 
+		void 
+	ResolveCombinedVelocities(
+		const MT_Vector3 & lin_vel,
+		const MT_Vector3 & ang_vel,
+		bool lin_vel_local,
+		bool ang_vel_local
+	);
 
 
 	/**
diff --git a/source/gameengine/Ketsji/KX_IPhysicsController.h b/source/gameengine/Ketsji/KX_IPhysicsController.h
index 0ed8cf1ad7d..d2f1f59bac8 100644
--- a/source/gameengine/Ketsji/KX_IPhysicsController.h
+++ b/source/gameengine/Ketsji/KX_IPhysicsController.h
@@ -68,6 +68,8 @@ public:
 	virtual MT_Vector3 GetVelocity(const MT_Point3& pos)=0;
 	virtual void	SetAngularVelocity(const MT_Vector3& ang_vel,bool local)=0;
 	virtual void	SetLinearVelocity(const MT_Vector3& lin_vel,bool local)=0;
+	virtual void	resolveCombinedVelocities(const MT_Vector3 & lin_vel, const MT_Vector3 & ang_vel) = 0;
+
 	virtual	void	getOrientation(MT_Quaternion& orn)=0;
 	virtual	void setOrientation(const MT_Quaternion& orn)=0;
 	virtual	void setPosition(const MT_Point3& pos)=0;
diff --git a/source/gameengine/Ketsji/KX_ObjectActuator.cpp b/source/gameengine/Ketsji/KX_ObjectActuator.cpp
index 1d913fe6e44..1f4e61f8139 100644
--- a/source/gameengine/Ketsji/KX_ObjectActuator.cpp
+++ b/source/gameengine/Ketsji/KX_ObjectActuator.cpp
@@ -82,20 +82,13 @@ bool KX_ObjectActuator::Update(double curtime,double deltatime)
 		// it should reconcile the externally set velocity with it's 
 		// own velocity.
 		if (m_active_combined_velocity) {
-			static bool update_resolve_warning = 0;
-			if (!update_resolve_warning) {
-				update_resolve_warning = 1;
-				std::cout << "FIXME: KX_ObjectActuator::Update ResolveCombinedVelocities undefined!" << std::endl;
-			}
-			//if (parent->GetSumoObject()) {
-				//parent->GetPhysicsController()->ResolveCombinedVelocities(
-				//	m_linear_velocity,
-				//	m_angular_velocity,
-				//	(m_bitLocalFlag.LinearVelocity) != 0,
-				//	(m_bitLocalFlag.AngularVelocity) != 0
-				//);
-				m_active_combined_velocity = false;
-			//}
+			parent->ResolveCombinedVelocities(
+					m_linear_velocity,
+					m_angular_velocity,
+					(m_bitLocalFlag.LinearVelocity) != 0,
+					(m_bitLocalFlag.AngularVelocity) != 0
+				);
+			m_active_combined_velocity = false;
 			return false;
 		} else {
 			return false; 
diff --git a/source/gameengine/Ketsji/KX_OdePhysicsController.cpp b/source/gameengine/Ketsji/KX_OdePhysicsController.cpp
index 0eff54dc0c6..e6c135b6aea 100644
--- a/source/gameengine/Ketsji/KX_OdePhysicsController.cpp
+++ b/source/gameengine/Ketsji/KX_OdePhysicsController.cpp
@@ -225,6 +225,9 @@ SG_Controller*	KX_OdePhysicsController::GetReplica(class SG_Node* destnode)
 	
 }
 
+void	KX_OdePhysicsController::resolveCombinedVelocities(const MT_Vector3 & lin_vel, const MT_Vector3 & ang_vel )
+{
+}
 
 	
 void	KX_OdePhysicsController::SetSumoTransform(bool nondynaonly)
diff --git a/source/gameengine/Ketsji/KX_OdePhysicsController.h b/source/gameengine/Ketsji/KX_OdePhysicsController.h
index 641a335d807..69c06a2b620 100644
--- a/source/gameengine/Ketsji/KX_OdePhysicsController.h
+++ b/source/gameengine/Ketsji/KX_OdePhysicsController.h
@@ -65,6 +65,7 @@ public:
 	virtual MT_Vector3 GetVelocity(const MT_Point3& pos);
 	virtual void	SetAngularVelocity(const MT_Vector3& ang_vel,bool local);
 	virtual void	SetLinearVelocity(const MT_Vector3& lin_vel,bool local);
+	virtual void	resolveCombinedVelocities(const MT_Vector3 & lin_vel, const MT_Vector3 & ang_vel );
 	virtual	void		getOrientation(MT_Quaternion& orn);
 	virtual	void setOrientation(const MT_Quaternion& orn);
 	virtual	void setPosition(const MT_Point3& pos);
diff --git a/source/gameengine/Ketsji/KX_SumoPhysicsController.cpp b/source/gameengine/Ketsji/KX_SumoPhysicsController.cpp
index 57986cd7b78..33ede321f02 100644
--- a/source/gameengine/Ketsji/KX_SumoPhysicsController.cpp
+++ b/source/gameengine/Ketsji/KX_SumoPhysicsController.cpp
@@ -24,17 +24,17 @@ void	KX_SumoPhysicsController::RelativeTranslate(const MT_Vector3& dloc,bool loc
 }
 void	KX_SumoPhysicsController::RelativeRotate(const MT_Matrix3x3& drot,bool local)
 {
-	double oldmat[12];
+	float oldmat[12];
 	drot.getValue(oldmat);
-	float newmat[9];
+/*	float newmat[9];
 	float *m = &newmat[0];
 	double *orgm = &oldmat[0];
 
 	 *m++ = *orgm++;*m++ = *orgm++;*m++ = *orgm++;orgm++;
 	 *m++ = *orgm++;*m++ = *orgm++;*m++ = *orgm++;orgm++;
-	 *m++ = *orgm++;*m++ = *orgm++;*m++ = *orgm++;orgm++;
+	 *m++ = *orgm++;*m++ = *orgm++;*m++ = *orgm++;orgm++; */
 
-	SumoPhysicsController::RelativeRotate(newmat,local);
+	SumoPhysicsController::RelativeRotate(oldmat,local);
 }
 
 void	KX_SumoPhysicsController::SetLinearVelocity(const MT_Vector3& lin_vel,bool local)
@@ -62,6 +62,15 @@ MT_Vector3 KX_SumoPhysicsController::GetLinearVelocity()
 	return GetVelocity(MT_Point3(0,0,0));
 	
 }
+
+void KX_SumoPhysicsController::resolveCombinedVelocities(
+		const MT_Vector3 & lin_vel,
+		const MT_Vector3 & ang_vel
+	)
+{
+	SumoPhysicsController::resolveCombinedVelocities(lin_vel, ang_vel);
+}
+
 void	KX_SumoPhysicsController::ApplyTorque(const MT_Vector3& torque,bool local)
 {
 	SumoPhysicsController::ApplyTorque(torque[0],torque[1],torque[2],local);
diff --git a/source/gameengine/Ketsji/KX_SumoPhysicsController.h b/source/gameengine/Ketsji/KX_SumoPhysicsController.h
index 465dd799c2a..dc038a536e9 100644
--- a/source/gameengine/Ketsji/KX_SumoPhysicsController.h
+++ b/source/gameengine/Ketsji/KX_SumoPhysicsController.h
@@ -76,6 +76,8 @@ public:
 	MT_Vector3 GetVelocity(const MT_Point3& pos);
 	void	SetAngularVelocity(const MT_Vector3& ang_vel,bool local);
 	void	SetLinearVelocity(const MT_Vector3& lin_vel,bool local);
+	void	resolveCombinedVelocities(const MT_Vector3 & lin_vel, const MT_Vector3 & ang_vel);
+
 
 	void	SuspendDynamics();
 	void	RestoreDynamics();
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
index aec4a410e1f..de9d2e20044 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
+++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
@@ -267,7 +267,7 @@ private:
 	actualAngVelocity(
 	) const ;
 	
-	bool dynamicCollision(const MT_Point3 &local2, 
+	void dynamicCollision(const MT_Point3 &local2, 
 		const MT_Vector3 &normal, 
 		MT_Scalar dist, 
 		const MT_Vector3 &rel_vel,
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
index cdf1a77adb3..ee8a7da953c 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
+++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
@@ -160,7 +160,7 @@ integrateMomentum(
 	}
 }
 
-bool SM_Object::dynamicCollision(const MT_Point3 &local2, 
+void SM_Object::dynamicCollision(const MT_Point3 &local2, 
 	const MT_Vector3 &normal, 
 	MT_Scalar dist, 
 	const MT_Vector3 &rel_vel,
@@ -297,7 +297,6 @@ bool SM_Object::dynamicCollision(const MT_Point3 &local2,
 		notifyClient();
 
 	}
-	return false;
 }
 
 DT_Bool SM_Object::boing(
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
index 0171d5f265e..38c2b48e460 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
+++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp
@@ -303,8 +303,13 @@ DT_Bool SM_Scene::boing(
 
 SM_Scene::~SM_Scene()
 { 
-/*	if (m_objectList.begin() != m_objectList.end())
-		std::cout << "SM_Scene::~SM_Scene: There are still objects in the Sumo scene!" << std::endl; */
+//	if (m_objectList.begin() != m_objectList.end()) 
+//		std::cout << "SM_Scene::~SM_Scene: There are still objects in the Sumo scene!" << std::endl;
+	for (T_ObjectList::iterator it = m_objectList.begin() ; it != m_objectList.end() ; it++)
+		delete *it;
+	
 	DT_DestroyRespTable(m_respTable);
+	DT_DestroyRespTable(m_secondaryRespTable);
+	DT_DestroyRespTable(m_fixRespTable);
 	DT_DestroyScene(m_scene);
 }
diff --git a/source/gameengine/Physics/Sumo/SumoPhysicsController.cpp b/source/gameengine/Physics/Sumo/SumoPhysicsController.cpp
index 76f5f6e891d..c08c20cf9b9 100644
--- a/source/gameengine/Physics/Sumo/SumoPhysicsController.cpp
+++ b/source/gameengine/Physics/Sumo/SumoPhysicsController.cpp
@@ -169,7 +169,7 @@ void		SumoPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlo
 	}
 
 }
-void		SumoPhysicsController::RelativeRotate(const float drot[9],bool local)
+void		SumoPhysicsController::RelativeRotate(const float drot[12],bool local)
 {
 
 	if (m_sumoObj )
@@ -185,9 +185,10 @@ void		SumoPhysicsController::RelativeRotate(const float drot[9],bool local)
 }
 void		SumoPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
 {
-	float orn [4]={quatImag0,quatImag1,quatImag2,quatReal};
-	MT_Quaternion quat(orn);
-	m_sumoObj->setOrientation(orn);
+//	float orn [4]={quatImag0,quatImag1,quatImag2,quatReal};
+//	MT_Quaternion quat;
+	m_sumoObj->setOrientation(MT_Quaternion(quatImag0,quatImag1,quatImag2,quatReal));
+	
 }
 
 void		SumoPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
@@ -271,6 +272,16 @@ void		SumoPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,flo
 	}
 }
 
+void SumoPhysicsController::resolveCombinedVelocities(
+		const MT_Vector3 & lin_vel,
+		const MT_Vector3 & ang_vel
+	)
+{
+	if (m_sumoObj)
+		m_sumoObj->resolveCombinedVelocities(lin_vel, ang_vel);
+}
+
+
 void		SumoPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
 {
 	if (m_sumoObj)
diff --git a/source/gameengine/Physics/Sumo/SumoPhysicsController.h b/source/gameengine/Physics/Sumo/SumoPhysicsController.h
index 4dd12b8b291..e1ce205b667 100644
--- a/source/gameengine/Physics/Sumo/SumoPhysicsController.h
+++ b/source/gameengine/Physics/Sumo/SumoPhysicsController.h
@@ -61,7 +61,7 @@ public:
 	
 		// kinematic methods
 	virtual void		RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local);
-	virtual void		RelativeRotate(const float drot[9],bool local);
+	virtual void		RelativeRotate(const float drot[12],bool local);
 	virtual	void		getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal);
 	virtual	void		setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal);
 	virtual	void		setPosition(float posX,float posY,float posZ);
@@ -72,6 +72,7 @@ public:
 	virtual void		ApplyForce(float forceX,float forceY,float forceZ,bool local);
 	virtual void		SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local);
 	virtual void		SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local);
+	virtual void		resolveCombinedVelocities(const MT_Vector3 & lin_vel, const MT_Vector3 & ang_vel );
 	virtual void		applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ);
 	virtual void		SetActive(bool active){};
 	virtual void		SuspendDynamics();
diff --git a/source/gameengine/Physics/common/PHY_IPhysicsController.h b/source/gameengine/Physics/common/PHY_IPhysicsController.h
index 69dbd6bfa60..8c94083af83 100644
--- a/source/gameengine/Physics/common/PHY_IPhysicsController.h
+++ b/source/gameengine/Physics/common/PHY_IPhysicsController.h
@@ -32,6 +32,8 @@
 #ifndef PHY_IPHYSICSCONTROLLER_H
 #define PHY_IPHYSICSCONTROLLER_H
 
+#include "MT_Vector3.h"
+
 /**
 	PHY_IPhysicsController is the abstract simplified Interface to a physical object.
 	It contains the IMotionState and IDeformableMesh Interfaces.
@@ -70,6 +72,7 @@ class PHY_IPhysicsController
 		virtual void		ApplyForce(float forceX,float forceY,float forceZ,bool local)=0;
 		virtual void		SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)=0;
 		virtual void		SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)=0;
+		virtual void		resolveCombinedVelocities(const MT_Vector3 & lin_vel, const MT_Vector3 & ang_vel ) = 0;
 		virtual void		applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)=0;
 		virtual void		SetActive(bool active)=0;
 
diff --git a/source/gameengine/Physics/common/SConscript b/source/gameengine/Physics/common/SConscript
index 07a1cea766b..6e4989f9a66 100755
--- a/source/gameengine/Physics/common/SConscript
+++ b/source/gameengine/Physics/common/SConscript
@@ -8,7 +8,8 @@ source_files = ['PHY_IMotionState.cpp',
                 'PHY_IPhysicsEnvironment.cpp']
 
 phy_physics_env.Append (CPPPATH = ['.',
-                                   '../Dummy'
+                                   '../Dummy',
+                                   '#intern/moto/include'
                                   ])
 
 phy_physics_env.Library (target='#'+user_options_dict['BUILD_DIR']+'/lib/PHY_Physics', source=source_files)