BGE #18665: Servo control and relative motion

Servo control motion actuator did not work as expected when the object
is moving on a moving platform. 

This patch introduces a new Ref field in the servo motion actuator
to set a reference object for the velocity calculation.
You can set the object during the game using the actuator "reference"
attribute; use an object name or an object reference.

The servo controller takes into account the angular velocity of the
reference object to compute the relative local velocity.

4 files changed, 100 insertions, 4 deletions

diff --git a/source/gameengine/Converter/KX_ConvertActuators.cpp b/source/gameengine/Converter/KX_ConvertActuators.cpp
index 60d64621665..98df34aa4a9 100644
--- a/source/gameengine/Converter/KX_ConvertActuators.cpp
+++ b/source/gameengine/Converter/KX_ConvertActuators.cpp
@@ -134,6 +134,7 @@ void BL_ConvertActuators(char* maggiename,
 		case ACT_OBJECT:
 			{
 				bObjectActuator* obact = (bObjectActuator*) bact->data;
+				KX_GameObject* obref = NULL;
 				MT_Vector3 forcevec(KX_BLENDERTRUNC(obact->forceloc[0]),
 					KX_BLENDERTRUNC(obact->forceloc[1]),
 					KX_BLENDERTRUNC(obact->forceloc[2]));
@@ -171,9 +172,13 @@ void BL_ConvertActuators(char* maggiename,
 				bitLocalFlag.AngularVelocity = bool((obact->flag & ACT_ANG_VEL_LOCAL)!=0);
 				bitLocalFlag.ServoControl = bool(obact->type == ACT_OBJECT_SERVO);
 				bitLocalFlag.AddOrSetLinV = bool((obact->flag & ACT_ADD_LIN_VEL)!=0);
-				
+				if (obact->reference && bitLocalFlag.ServoControl)
+				{
+					obref = converter->FindGameObject(obact->reference);
+				}
 				
 				KX_ObjectActuator* tmpbaseact = new KX_ObjectActuator(gameobj,
+					obref,
 					forcevec.getValue(),
 					torquevec.getValue(),
 					dlocvec.getValue(),
diff --git a/source/gameengine/Ketsji/KX_ObjectActuator.cpp b/source/gameengine/Ketsji/KX_ObjectActuator.cpp
index 0232e3407b0..eaae04d406d 100644
--- a/source/gameengine/Ketsji/KX_ObjectActuator.cpp
+++ b/source/gameengine/Ketsji/KX_ObjectActuator.cpp
@@ -45,6 +45,7 @@
 KX_ObjectActuator::
 KX_ObjectActuator(
 	SCA_IObject* gameobj,
+	KX_GameObject* refobj,
 	const MT_Vector3& force,
 	const MT_Vector3& torque,
 	const MT_Vector3& dloc,
@@ -69,6 +70,7 @@ KX_ObjectActuator(
 	m_previous_error(0.0,0.0,0.0),
 	m_error_accumulator(0.0,0.0,0.0),
 	m_bitLocalFlag (flag),
+	m_reference(refobj),
 	m_active_combined_velocity (false),
 	m_linear_damping_active(false),
 	m_angular_damping_active(false)
@@ -80,10 +82,17 @@ KX_ObjectActuator(
 
 		m_pid = m_torque;
 	}
-
+	if (m_reference)
+		m_reference->RegisterActuator(this);
 	UpdateFuzzyFlags();
 }
 
+KX_ObjectActuator::~KX_ObjectActuator()
+{
+	if (m_reference)
+		m_reference->UnregisterActuator(this);
+}
+
 bool KX_ObjectActuator::Update()
 {
 	
@@ -132,6 +141,18 @@ bool KX_ObjectActuator::Update()
 			if (mass < MT_EPSILON)
 				return false;
 			MT_Vector3 v = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
+			if (m_reference)
+			{
+				const MT_Point3& mypos = parent->NodeGetWorldPosition();
+				const MT_Point3& refpos = m_reference->NodeGetWorldPosition();
+				MT_Point3 relpos;
+				relpos = (mypos-refpos);
+				MT_Vector3 vel= m_reference->GetVelocity(relpos);
+				if (m_bitLocalFlag.LinearVelocity)
+					// must convert in local space
+					vel = parent->NodeGetWorldOrientation().transposed()*vel;
+				v -= vel;
+			}
 			MT_Vector3 e = m_linear_velocity - v;
 			MT_Vector3 dv = e - m_previous_error;
 			MT_Vector3 I = m_error_accumulator + e;
@@ -260,7 +281,34 @@ CValue* KX_ObjectActuator::GetReplica()
 	return replica;
 }
 
+void KX_ObjectActuator::ProcessReplica()
+{
+	SCA_IActuator::ProcessReplica();
+	if (m_reference)
+		m_reference->RegisterActuator(this);
+}
 
+bool KX_ObjectActuator::UnlinkObject(SCA_IObject* clientobj)
+{
+	if (clientobj == (SCA_IObject*)m_reference)
+	{
+		// this object is being deleted, we cannot continue to use it as reference.
+		m_reference = NULL;
+		return true;
+	}
+	return false;
+}
+
+void KX_ObjectActuator::Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map)
+{
+	void **h_obj = (*obj_map)[m_reference];
+	if (h_obj) {
+		if (m_reference)
+			m_reference->UnregisterActuator(this);
+		m_reference = (KX_GameObject*)(*h_obj);
+		m_reference->RegisterActuator(this);
+	}
+}
 
 /* some 'standard' utilities... */
 bool KX_ObjectActuator::isValid(KX_ObjectActuator::KX_OBJECT_ACT_VEC_TYPE type)
@@ -357,6 +405,7 @@ PyAttributeDef KX_ObjectActuator::Attributes[] = {
 	KX_PYATTRIBUTE_RW_FUNCTION("forceLimitY", KX_ObjectActuator, pyattr_get_forceLimitY, pyattr_set_forceLimitY),
 	KX_PYATTRIBUTE_RW_FUNCTION("forceLimitZ", KX_ObjectActuator, pyattr_get_forceLimitZ, pyattr_set_forceLimitZ),
 	KX_PYATTRIBUTE_VECTOR_RW_CHECK("pid", -100, 200, true, KX_ObjectActuator, m_pid, PyCheckPid),
+	KX_PYATTRIBUTE_RW_FUNCTION("reference", KX_ObjectActuator,pyattr_get_reference,pyattr_set_reference),
 	{ NULL }	//Sentinel
 };
 
@@ -484,6 +533,38 @@ int	KX_ObjectActuator::pyattr_set_forceLimitZ(void *self_v, const KX_PYATTRIBUTE
 	return PY_SET_ATTR_FAIL;
 }
 
+PyObject* KX_ObjectActuator::pyattr_get_reference(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	KX_ObjectActuator* actuator = static_cast<KX_ObjectActuator*>(self);
+	if (!actuator->m_reference)
+		Py_RETURN_NONE;
+	
+	return actuator->m_reference->GetProxy();
+}
+
+int KX_ObjectActuator::pyattr_set_reference(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
+{
+	KX_ObjectActuator* actuator = static_cast<KX_ObjectActuator*>(self);
+	KX_GameObject *refOb;
+	
+	if (!ConvertPythonToGameObject(value, &refOb, true, "actu.reference = value: KX_ObjectActuator"))
+		return PY_SET_ATTR_FAIL;
+	
+	if (actuator->m_reference)
+		actuator->m_reference->UnregisterActuator(actuator);
+	
+	if(refOb==NULL) {
+		actuator->m_reference= NULL;
+	}
+	else {	
+		actuator->m_reference = refOb;
+		actuator->m_reference->RegisterActuator(actuator);
+	}
+	
+	return PY_SET_ATTR_SUCCESS;
+}
+
+
 /* 1. set ------------------------------------------------------------------ */
 /* Removed! */
 
diff --git a/source/gameengine/Ketsji/KX_ObjectActuator.h b/source/gameengine/Ketsji/KX_ObjectActuator.h
index b60f877074d..f9bd2a0c748 100644
--- a/source/gameengine/Ketsji/KX_ObjectActuator.h
+++ b/source/gameengine/Ketsji/KX_ObjectActuator.h
@@ -35,6 +35,8 @@
 #include "SCA_IActuator.h"
 #include "MT_Vector3.h"
 
+class KX_GameObject;
+
 //
 // Stores the flags for each CValue derived class
 //
@@ -92,7 +94,7 @@ class KX_ObjectActuator : public SCA_IActuator
 	MT_Vector3		m_previous_error;
 	MT_Vector3		m_error_accumulator;
   	KX_LocalFlags	m_bitLocalFlag;
-
+	KX_GameObject*  m_reference;
 	// A hack bool -- oh no sorry everyone
 	// This bool is used to check if we have informed 
 	// the physics object that we are no longer 
@@ -121,6 +123,7 @@ public:
 
 	KX_ObjectActuator(
 		SCA_IObject* gameobj,
+		KX_GameObject* refobj,
 		const MT_Vector3& force,
 		const MT_Vector3& torque,
 		const MT_Vector3& dloc,
@@ -131,8 +134,11 @@ public:
 		const KX_LocalFlags& flag,
 		PyTypeObject* T=&Type
 	);
-
+	~KX_ObjectActuator();
 	CValue* GetReplica();
+	void ProcessReplica();
+	bool UnlinkObject(SCA_IObject* clientobj);
+	void Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map);
 
 	void SetForceLoc(const double force[3])	{ /*m_force=force;*/ }
 	void UpdateFuzzyFlags()
@@ -188,6 +194,8 @@ public:
 	static int			pyattr_set_forceLimitY(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef, PyObject *value);
 	static PyObject*	pyattr_get_forceLimitZ(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef);
 	static int			pyattr_set_forceLimitZ(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef, PyObject *value);
+	static PyObject*	pyattr_get_reference(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+	static int			pyattr_set_reference(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value);
 
 	// This lets the attribute macros use UpdateFuzzyFlags()
 	static int PyUpdateFuzzyFlags(void *self, const PyAttributeDef *attrdef)
diff --git a/source/gameengine/PyDoc/GameTypes.py b/source/gameengine/PyDoc/GameTypes.py
index ca02b1a2798..5ec6f31e30c 100644
--- a/source/gameengine/PyDoc/GameTypes.py
+++ b/source/gameengine/PyDoc/GameTypes.py
@@ -2668,6 +2668,8 @@ class KX_ObjectActuator(SCA_IActuator):
 	
 	@ivar pid: The PID coefficients of the servo controller
 	@type pid: list of floats [proportional, integral, derivate]
+	@ivar reference: The object that is used as reference to compute the velocity for the servo controller.
+	@type reference: KX_GameObject or None
 	
 	@group Deprecated: getForce, setForce, getTorque, setTorque, getDLoc, setDLoc, getDRot, setDRot, getLinearVelocity, setLinearVelocity, getAngularVelocity,
 						setAngularVelocity, getDamping, setDamping, getForceLimitX, setForceLimitX, getForceLimitY, setForceLimitY, getForceLimitZ, setForceLimitZ,