Skip to content

Commit

Permalink
UPBGE: Implement angular servo control motion logic brick.
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@panzergame
panzergame committed Oct 19, 2017
1 parent 846d96f commit 3ca1d2d
Show file tree
Hide file tree
Showing 6 changed files with 91 additions and 46 deletions.
16 changes: 13 additions & 3 deletions source/blender/editors/space_logic/logic_window.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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")) {
Expand Down Expand Up @@ -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);
Expand Down
6 changes: 6 additions & 0 deletions source/blender/makesdna/DNA_actuator_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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
Expand Down
17 changes: 14 additions & 3 deletions source/blender/makesrna/intern/rna_actuator.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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");
Expand Down Expand Up @@ -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);
Expand Down
1 change: 1 addition & 0 deletions source/gameengine/Converter/BL_ConvertActuators.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
96 changes: 56 additions & 40 deletions source/gameengine/Ketsji/KX_ObjectActuator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
1 change: 1 addition & 0 deletions source/gameengine/Ketsji/KX_ObjectActuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,7 @@ struct KX_LocalFlags {
bool ZeroDLoc;
bool ZeroLinearVelocity;
bool ZeroAngularVelocity;
bool ServoControlAngular;
};

class KX_ObjectActuator : public SCA_IActuator
Expand Down

0 comments on commit 3ca1d2d

Please sign in to comment.