Added resolveCombinedVelocities()

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

8 files changed, 51 insertions, 22 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();