UPBGE: Implement angular servo control motion logic brick.

Previously the servo control logic brick was able to control only
the linear velocity using force. But some users showed that in some
non-rare case it could be needed to servo control the angular velocity
via torques.

To fill this gap, the servo control logic brick exposes a enum named
"Servo Type" to select either linear or angular velocity, linear is
the default. This option is used in KX_ObjectActuator to compute
from linear or angular velocity a force or torque.

Note: The reference object in case of angular velocity is just used to
substract its angular velocity contrary to linear velocity which compute
the linear velocity of the reference object on the position of the affected
object.

Author: panzergame

source/blender/editors/space_logic/logic_window.c

@@ -1741,11 +1741,13 @@ static void draw_actuator_motion(uiLayout *layout, PointerRNA *ptr)
 	PointerRNA settings_ptr;
 	uiLayout *split, *row, *col, *sub;
 	int physics_type;
+	bool angular;
 
 	ob = (Object *)ptr->id.data;
 	RNA_pointer_create((ID *)ob, &RNA_GameObjectSettings, ob, &settings_ptr);
 	physics_type = RNA_enum_get(&settings_ptr, "physics_type");
-	
+	angular = (RNA_enum_get(ptr, "servo_mode") == ACT_SERVO_ANGULAR);
+
 	uiItemR(layout, ptr, "mode", 0, NULL, ICON_NONE);
 
 	switch (RNA_enum_get(ptr, "mode")) {
@@ -1790,10 +1792,18 @@ static void draw_actuator_motion(uiLayout *layout, PointerRNA *ptr)
 		case ACT_OBJECT_SERVO:
 			uiItemR(layout, ptr, "reference_object", 0, NULL, ICON_NONE);
 
+			uiItemR(layout, ptr, "servo_mode", 0, NULL, ICON_NONE);
+
 			split = uiLayoutSplit(layout, 0.9, false);
 			row = uiLayoutRow(split, false);
-			uiItemR(row, ptr, "linear_velocity", 0, NULL, ICON_NONE);
-			uiItemR(split, ptr, "use_local_linear_velocity", UI_ITEM_R_TOGGLE, NULL, ICON_NONE);
+			if (angular) {
+				uiItemR(row, ptr, "angular_velocity", 0, NULL, ICON_NONE);
+				uiItemR(split, ptr, "use_local_angular_velocity", UI_ITEM_R_TOGGLE, NULL, ICON_NONE);
+			}
+			else {
+				uiItemR(row, ptr, "linear_velocity", 0, NULL, ICON_NONE);
+				uiItemR(split, ptr, "use_local_linear_velocity", UI_ITEM_R_TOGGLE, NULL, ICON_NONE);
+			}
 
 			row = uiLayoutRow(layout, false);
 			col = uiLayoutColumn(row, false);

source/blender/makesdna/DNA_actuator_types.h

@@ -117,6 +117,8 @@ typedef struct bPropertyActuator {
 typedef struct bObjectActuator {
 	short flag, type, otype;
 	short damping;
+	short servotype;
+	short pad2[3];
 	float forceloc[3], forcerot[3];
 	float pad[3], pad1[3];
 	float dloc[3], drot[3]; /* angle in radians */
@@ -308,6 +310,10 @@ typedef struct bActuator {
 #define ACT_OBJECT_SERVO		1
 #define ACT_OBJECT_CHARACTER	2
 
+/* objectactuator->servotype */
+#define ACT_SERVO_LINEAR	0
+#define ACT_SERVO_ANGULAR	1
+
 /* actuator->type */
 #define ACT_OBJECT		0
 #define ACT_IPO			1

source/blender/makesrna/intern/rna_actuator.c

@@ -725,6 +725,12 @@ static void rna_def_object_actuator(BlenderRNA *brna)
 		{0, NULL, 0, NULL, NULL}
 	};
 
+	static EnumPropertyItem prop_servo_type_items[] = {
+		{ACT_SERVO_LINEAR, "SERVO_LINEAR", 0, "Linear", ""},
+		{ACT_SERVO_ANGULAR, "SERVO_ANGULAR", 0, "Angular", ""},
+		{0, NULL, 0, NULL, NULL}
+	};
+
 	srna = RNA_def_struct(brna, "ObjectActuator", "Actuator");
 	RNA_def_struct_ui_text(srna, "Motion Actuator", "Actuator to control the object movement");
 	RNA_def_struct_sdna_from(srna, "bObjectActuator", "data");
@@ -736,6 +742,11 @@ static void rna_def_object_actuator(BlenderRNA *brna)
 	RNA_def_property_enum_funcs(prop, NULL, "rna_ObjectActuator_type_set", NULL);
 	RNA_def_property_ui_text(prop, "Motion Type", "Specify the motion system");
 	RNA_def_property_update(prop, NC_LOGIC, NULL);
+
+	prop = RNA_def_property(srna, "servo_mode", PROP_ENUM, PROP_NONE);
+	RNA_def_property_enum_sdna(prop, NULL, "servotype");
+	RNA_def_property_enum_items(prop, prop_servo_type_items);
+	RNA_def_property_ui_text(prop, "Servo Type", "Specify the servo control system");
 
 	prop = RNA_def_property(srna, "reference_object", PROP_POINTER, PROP_NONE);
 	RNA_def_property_struct_type(prop, "Object");
@@ -895,17 +906,17 @@ static void rna_def_object_actuator(BlenderRNA *brna)
 
 	prop = RNA_def_property(srna, "use_servo_limit_x", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "flag", ACT_SERVO_LIMIT_X);
-	RNA_def_property_ui_text(prop, "X", "Set limit to force along the X axis");
+	RNA_def_property_ui_text(prop, "X", "Set limit to force/torque along the X axis");
 	RNA_def_property_update(prop, NC_LOGIC, NULL);
 
 	prop = RNA_def_property(srna, "use_servo_limit_y", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "flag", ACT_SERVO_LIMIT_Y);
-	RNA_def_property_ui_text(prop, "Y", "Set limit to force along the Y axis");
+	RNA_def_property_ui_text(prop, "Y", "Set limit to force/torque along the Y axis");
 	RNA_def_property_update(prop, NC_LOGIC, NULL);
 
 	prop = RNA_def_property(srna, "use_servo_limit_z", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "flag", ACT_SERVO_LIMIT_Z);
-	RNA_def_property_ui_text(prop, "Z", "Set limit to force along the Z axis");
+	RNA_def_property_ui_text(prop, "Z", "Set limit to force/torque along the Z axis");
 	RNA_def_property_update(prop, NC_LOGIC, NULL);
 
 	prop = RNA_def_property(srna, "use_character_jump", PROP_BOOLEAN, PROP_NONE);

source/gameengine/Converter/BL_ConvertActuators.cpp

@@ -185,6 +185,7 @@ void BL_ConvertActuators(const char* maggiename,
 				bitLocalFlag.CharacterJump = bool((obact->flag & ACT_CHAR_JUMP)!=0);
 				bitLocalFlag.AddOrSetLinV = bool((obact->flag & ACT_ADD_LIN_VEL)!=0);
 				bitLocalFlag.AddOrSetCharLoc = bool((obact->flag & ACT_ADD_CHAR_LOC)!=0);
+				bitLocalFlag.ServoControlAngular = (obact->servotype == ACT_SERVO_ANGULAR);
 				if (obact->reference && bitLocalFlag.ServoControl)
 				{
 					obref = converter.FindGameObject(obact->reference);

source/gameengine/Ketsji/KX_ObjectActuator.cpp

@@ -148,59 +148,75 @@ bool KX_ObjectActuator::Update()
 			if (mass < MT_EPSILON) {
 				return false;
 			}
-			MT_Vector3 v = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
+
+			MT_Vector3 v;
+			if (m_bitLocalFlag.ServoControlAngular) {
+				v = parent->GetAngularVelocity(m_bitLocalFlag.AngularVelocity);
+			}
+			else {
+				v = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
+			}
+
 			if (m_reference) {
-				const MT_Vector3& mypos = parent->NodeGetWorldPosition();
-				const MT_Vector3& refpos = m_reference->NodeGetWorldPosition();
-				MT_Vector3 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;
+				if (m_bitLocalFlag.ServoControlAngular) {
+					const MT_Vector3 vel = m_reference->GetAngularVelocity(m_bitLocalFlag.AngularVelocity);
+					v -= vel;
+				}
+				else {
+					const MT_Vector3& mypos = parent->NodeGetWorldPosition();
+					const MT_Vector3& refpos = m_reference->NodeGetWorldPosition();
+					const MT_Vector3 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;
 				}
-				v -= vel;
 			}
-			MT_Vector3 e = m_linear_velocity - v;
+
+			MT_Vector3 e;
+			if (m_bitLocalFlag.ServoControlAngular) {
+				e = m_angular_velocity - v;
+			}
+			else {
+				e = m_linear_velocity - v;
+			}
+
 			MT_Vector3 dv = e - m_previous_error;
 			MT_Vector3 I = m_error_accumulator + e;
 
-			m_force = m_pid.x() * e + m_pid.y() * I + m_pid.z() * dv;
+			MT_Vector3& f = (m_bitLocalFlag.ServoControlAngular) ? m_force : m_torque;
+			f = m_pid.x() * e + m_pid.y() * I + m_pid.z() * dv;
 			// to automatically adapt the PID coefficient to mass;
-			m_force *= mass;
-			if (m_bitLocalFlag.Torque) {
-				if (m_force[0] > m_dloc[0]) {
-					m_force[0] = m_dloc[0];
-					I[0] = m_error_accumulator[0];
-				}
-				else if (m_force[0] < m_drot[0]) {
-					m_force[0] = m_drot[0];
-					I[0] = m_error_accumulator[0];
-				}
-			}
-			if (m_bitLocalFlag.DLoc) {
-				if (m_force[1] > m_dloc[1]) {
-					m_force[1] = m_dloc[1];
-					I[1] = m_error_accumulator[1];
-				}
-				else if (m_force[1] < m_drot[1]) {
-					m_force[1] = m_drot[1];
-					I[1] = m_error_accumulator[1];
+			f *= mass;
+
+			const bool limits[3] = {m_bitLocalFlag.Torque, m_bitLocalFlag.DLoc, m_bitLocalFlag.DRot};
+
+			for (unsigned short i = 0; i <  3; ++i) {
+				if (!limits[i]) {
+					continue;
 				}
-			}
-			if (m_bitLocalFlag.DRot) {
-				if (m_force[2] > m_dloc[2]) {
-					m_force[2] = m_dloc[2];
-					I[2] = m_error_accumulator[2];
+
+				if (f[i] > m_dloc[i]) {
+					f[i] = m_dloc[i];
+					I[i] = m_error_accumulator[i];
 				}
-				else if (m_force[2] < m_drot[2]) {
-					m_force[2] = m_drot[2];
-					I[2] = m_error_accumulator[2];
+				else if (f[i] < m_drot[i]) {
+					f[i] = m_drot[i];
+					I[i] = m_error_accumulator[i];
 				}
 			}
+
 			m_previous_error = e;
 			m_error_accumulator = I;
-			parent->ApplyForce(m_force, (m_bitLocalFlag.LinearVelocity) != 0);
+
+			if (m_bitLocalFlag.ServoControlAngular) {
+				parent->ApplyTorque(f, m_bitLocalFlag.AngularVelocity);
+			}
+			else {
+				parent->ApplyForce(f, m_bitLocalFlag.LinearVelocity);
+			}
 		}
 		else if (m_bitLocalFlag.CharacterMotion) {
 			MT_Vector3 dir = m_dloc;

source/gameengine/Ketsji/KX_ObjectActuator.h

@@ -81,6 +81,7 @@ struct KX_LocalFlags {
 	bool ZeroDLoc;
 	bool ZeroLinearVelocity;
 	bool ZeroAngularVelocity;
+	bool ServoControlAngular;
 };
 
 class KX_ObjectActuator : public SCA_IActuator