Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ballhead constraints for FABRIK #62003

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
+109 −1
Open

Ballhead constraints for FABRIK #62003

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
4ec1608
Added ballhead constraint for bones
Jun 13, 2022
8f3ee79
Generated structure for documentation
Jun 13, 2022
ca608e5
Fixed naming conventions
Jun 13, 2022
86ab1cb
Appied revisions
Jun 13, 2022
9d7553c
Fixed out of index access which was not tested. Further corrections b…
Jun 13, 2022
83f28c6
Hopefully the final fix for the runtime checks.
Jun 13, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 15 additions & 0 deletions doc/classes/SkeletonModification3DFABRIK.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,12 @@
Returns the magnet vector of the FABRIK joint at [code]joint_idx[/code].
</description>
</method>
<method name="get_fabrik_joint_rotational_constraint" qualifiers="const">
<return type="float" />
<argument index="0" name="joint_idx" type="int" />
<description>
</description>
</method>
<method name="get_fabrik_joint_tip_node" qualifiers="const">
<return type="NodePath" />
<argument index="0" name="joint_idx" type="int" />
Expand Down Expand Up @@ -117,6 +123,13 @@
Sets the magenet position to [code]magnet_position[/code] for the joint at [code]joint_idx[/code]. The magnet position is used to nudge the joint in that direction when solving, which gives some control over how that joint will bend when being solved.
fire marked this conversation as resolved.
Show resolved Hide resolved
</description>
</method>
<method name="set_fabrik_joint_rotational_constraint">
<return type="void" />
<argument index="0" name="joint_idx" type="int" />
<argument index="1" name="rotational_constraint" type="float" />
<description>
</description>
</method>
<method name="set_fabrik_joint_tip_node">
<return type="void" />
<argument index="0" name="joint_idx" type="int" />
Expand Down Expand Up @@ -154,6 +167,8 @@
<member name="fabrik_data_chain_length" type="int" setter="set_fabrik_data_chain_length" getter="get_fabrik_data_chain_length" default="0">
The amount of FABRIK joints in the FABRIK modification.
</member>
<member name="limit_rotation" type="bool" setter="set_limit_rotation" getter="get_limit_rotation" default="false">
</member>
<member name="target_nodepath" type="NodePath" setter="set_target_node" getter="get_target_node" default="NodePath(&quot;&quot;)">
The NodePath to the node that is the target for the FABRIK modification. This node is what the FABRIK chain will attempt to rotate the bone chain to.
</member>
Expand Down
81 changes: 80 additions & 1 deletion scene/resources/skeleton_modification_3d_fabrik.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@ bool SkeletonModification3DFABRIK::_set(const StringName &p_path, const Variant
set_fabrik_joint_use_target_basis(which, p_value);
} else if (what == "roll") {
set_fabrik_joint_roll(which, Math::deg2rad(real_t(p_value)));
} else if (what == "rotational_constraint") {
set_fabrik_joint_rotational_constraint(which, Math::deg2rad(real_t(p_value)));
}
return true;
}
Expand Down Expand Up @@ -92,6 +94,8 @@ bool SkeletonModification3DFABRIK::_get(const StringName &p_path, Variant &r_ret
r_ret = get_fabrik_joint_use_target_basis(which);
} else if (what == "roll") {
r_ret = Math::rad2deg(get_fabrik_joint_roll(which));
} else if (what == "rotational_constraint") {
r_ret = Math::rad2deg(get_fabrik_joint_rotational_constraint(which));
}
return true;
}
Expand All @@ -105,6 +109,7 @@ void SkeletonModification3DFABRIK::_get_property_list(List<PropertyInfo> *p_list
p_list->push_back(PropertyInfo(Variant::STRING_NAME, base_string + "bone_name", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT));
p_list->push_back(PropertyInfo(Variant::INT, base_string + "bone_index", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT));
p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "roll", PROPERTY_HINT_RANGE, "-360,360,0.01", PROPERTY_USAGE_DEFAULT));
p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "rotational_constraint", PROPERTY_HINT_RANGE, "-180,180,0.01", PROPERTY_USAGE_DEFAULT));
p_list->push_back(PropertyInfo(Variant::BOOL, base_string + "auto_calculate_length", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT));

if (!fabrik_data_chain[i].auto_calculate_length) {
Expand Down Expand Up @@ -205,6 +210,50 @@ void SkeletonModification3DFABRIK::_execute(real_t p_delta) {
execution_error_found = false;
}

Vector3 SkeletonModification3DFABRIK::chain_ball_constraint(int i) {
// Get the inner-to-outer direction of this bone as well as the previous bone to use as a baseline
// Direction for the line L1 "Godot has a good function (direction_to) which does (B - A).normalized : A.direction_to(B)"
Vector3 current_bone_inout_direction = fabrik_transforms[i - 1].origin.direction_to(fabrik_transforms[i].origin);
Vector3 prev_bone_inout_direction = fabrik_transforms[i + 1].origin.direction_to(fabrik_transforms[i].origin);

real_t angle_between = get_angle_between(current_bone_inout_direction, prev_bone_inout_direction);
real_t constraint_angle = fabrik_data_chain[i].rotational_constraint;

if (angle_between > constraint_angle) {
return get_angle_limited_unit_vector(current_bone_inout_direction, prev_bone_inout_direction, constraint_angle);
} else {
return Vector3();
}
}

Vector3 SkeletonModification3DFABRIK::get_angle_limited_unit_vector(const Vector3 &vec_to_limit, const Vector3 &vec_baseline, real_t angle_limit) {
// Get the angle between the two vectors
// Note: This will ALWAYS be a positive value between 0 and Pi.
float angle_between = get_angle_between(vec_baseline, vec_to_limit);

if (angle_between > angle_limit) {
// The axis which we need to rotate around is the one perpendicular to the two vectors - so we're
// rotating around the vector which is the cross-product of our two vectors.
// Note: We do not have to worry about both vectors being the same or pointing in opposite directions
// because if they bones are the same direction they will not have an angle greater than the angle limit,
// and if they point opposite directions we will approach but not quite reach the precise max angle
// limit of Pi (I believe).
Vector3 correction_axis = (vec_baseline.normalized().cross(vec_to_limit.normalized())).normalized();

// Our new vector is the baseline vector rotated by the max allowable angle about the correction axis
return vec_baseline.rotated(correction_axis, Math::rad2deg(angle_limit)).normalized();
} else // Angle not greater than limit? Just return a normalised version of the vec_to_limit
{
// This may already BE normalised, but we have no way of knowing without calcing the length, so best be safe and normalise.
// TODO: If performance is an issue, then I could get the length, and if it's not approx. 1.0f THEN normalise otherwise just return as is.
return vec_to_limit.normalized();
}
}

real_t SkeletonModification3DFABRIK::get_angle_between(const Vector3 &vec1, const Vector3 &vec2) {
return Math::acos(vec1.normalized().dot(vec2.normalized()));
}

void SkeletonModification3DFABRIK::chain_backwards() {
int final_bone_idx = fabrik_data_chain[final_joint_idx].bone_idx;
Transform3D final_joint_trans = fabrik_transforms[final_joint_idx];
Expand Down Expand Up @@ -232,7 +281,13 @@ void SkeletonModification3DFABRIK::chain_backwards() {
Transform3D current_trans = fabrik_transforms[i];

real_t length = fabrik_data_chain[i].length / (current_trans.origin.distance_to(next_bone_trans.origin));
current_trans.origin = next_bone_trans.origin.lerp(current_trans.origin, length);

Vector3 new_point = Vector3();
if (limit_rotation && (i < final_joint_idx)) {
new_point = chain_ball_constraint(i);
}

current_trans.origin = next_bone_trans.origin.lerp(current_trans.origin + new_point, length);

// Save the result
fabrik_transforms[i] = current_trans;
Expand Down Expand Up @@ -381,6 +436,14 @@ void SkeletonModification3DFABRIK::set_chain_tolerance(real_t p_tolerance) {
chain_tolerance = p_tolerance;
}

bool SkeletonModification3DFABRIK::get_limit_rotation() const {
return limit_rotation;
}

void SkeletonModification3DFABRIK::set_limit_rotation(bool p_rot) {
limit_rotation = p_rot;
}

int SkeletonModification3DFABRIK::get_chain_max_iterations() {
return chain_max_iterations;
}
Expand Down Expand Up @@ -583,6 +646,17 @@ void SkeletonModification3DFABRIK::set_fabrik_joint_roll(int p_joint_idx, real_t
fabrik_data_chain[p_joint_idx].roll = p_roll;
}

real_t SkeletonModification3DFABRIK::get_fabrik_joint_rotational_constraint(int p_joint_idx) const {
return fabrik_data_chain[p_joint_idx].rotational_constraint;
}

void SkeletonModification3DFABRIK::set_fabrik_joint_rotational_constraint(int p_joint_idx, real_t p_rot) {
const int bone_chain_size = fabrik_data_chain.size();
ERR_FAIL_INDEX(p_joint_idx, bone_chain_size);
ERR_FAIL_COND_MSG(Math::abs(p_rot) > Math_PI, "Ball-head constraint must be limited to [-180, +180]");
fabrik_data_chain[p_joint_idx].rotational_constraint = p_rot;
}

void SkeletonModification3DFABRIK::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_target_node", "target_nodepath"), &SkeletonModification3DFABRIK::set_target_node);
ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonModification3DFABRIK::get_target_node);
Expand All @@ -592,6 +666,8 @@ void SkeletonModification3DFABRIK::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_chain_tolerance"), &SkeletonModification3DFABRIK::get_chain_tolerance);
ClassDB::bind_method(D_METHOD("set_chain_max_iterations", "max_iterations"), &SkeletonModification3DFABRIK::set_chain_max_iterations);
ClassDB::bind_method(D_METHOD("get_chain_max_iterations"), &SkeletonModification3DFABRIK::get_chain_max_iterations);
ClassDB::bind_method(D_METHOD("set_limit_rotation", "limit_rotation"), &SkeletonModification3DFABRIK::set_limit_rotation);
ClassDB::bind_method(D_METHOD("get_limit_rotation"), &SkeletonModification3DFABRIK::get_limit_rotation);

// FABRIK joint data functions
ClassDB::bind_method(D_METHOD("get_fabrik_joint_bone_name", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_bone_name);
Expand All @@ -611,10 +687,13 @@ void SkeletonModification3DFABRIK::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_fabrik_joint_tip_node", "joint_idx", "tip_node"), &SkeletonModification3DFABRIK::set_fabrik_joint_tip_node);
ClassDB::bind_method(D_METHOD("get_fabrik_joint_use_target_basis", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_use_target_basis);
ClassDB::bind_method(D_METHOD("set_fabrik_joint_use_target_basis", "joint_idx", "use_target_basis"), &SkeletonModification3DFABRIK::set_fabrik_joint_use_target_basis);
ClassDB::bind_method(D_METHOD("set_fabrik_joint_rotational_constraint", "joint_idx", "rotational_constraint"), &SkeletonModification3DFABRIK::set_fabrik_joint_rotational_constraint);
ClassDB::bind_method(D_METHOD("get_fabrik_joint_rotational_constraint", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_rotational_constraint);

ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_nodepath", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D"), "set_target_node", "get_target_node");
ADD_PROPERTY(PropertyInfo(Variant::INT, "fabrik_data_chain_length", PROPERTY_HINT_RANGE, "0,100,1"), "set_fabrik_data_chain_length", "get_fabrik_data_chain_length");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "chain_tolerance", PROPERTY_HINT_RANGE, "0,100,0.001"), "set_chain_tolerance", "get_chain_tolerance");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "limit_rotation"), "set_limit_rotation", "get_limit_rotation");
ADD_PROPERTY(PropertyInfo(Variant::INT, "chain_max_iterations", PROPERTY_HINT_RANGE, "1,50,1"), "set_chain_max_iterations", "get_chain_max_iterations");
}

Expand Down
12 changes: 12 additions & 0 deletions scene/resources/skeleton_modification_3d_fabrik.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,8 @@ class SkeletonModification3DFABRIK : public SkeletonModification3D {

bool use_target_basis = false;
real_t roll = 0;

real_t rotational_constraint = 0;
};

LocalVector<FABRIK_Joint_Data> fabrik_data_chain;
Expand All @@ -63,6 +65,7 @@ class SkeletonModification3DFABRIK : public SkeletonModification3D {
real_t chain_tolerance = 0.01;
int chain_max_iterations = 10;
int chain_iterations = 0;
bool limit_rotation = false;

void update_target_cache();
void update_joint_tip_cache(int p_joint_idx);
Expand All @@ -75,6 +78,10 @@ class SkeletonModification3DFABRIK : public SkeletonModification3D {
void chain_forwards();
void chain_apply();

Vector3 chain_ball_constraint(int i);
Vector3 get_angle_limited_unit_vector(const Vector3 &vec_to_limit, const Vector3 &vec_baseline, real_t angle_limit);
static real_t get_angle_between(const Vector3 &vec1, const Vector3 &vec2);

protected:
static void _bind_methods();
bool _get(const StringName &p_path, Variant &r_ret) const;
Expand All @@ -97,6 +104,9 @@ class SkeletonModification3DFABRIK : public SkeletonModification3D {
int get_chain_max_iterations();
void set_chain_max_iterations(int p_iterations);

bool get_limit_rotation() const;
void set_limit_rotation(bool p_rot);

String get_fabrik_joint_bone_name(int p_joint_idx) const;
void set_fabrik_joint_bone_name(int p_joint_idx, String p_bone_name);
int get_fabrik_joint_bone_index(int p_joint_idx) const;
Expand All @@ -116,6 +126,8 @@ class SkeletonModification3DFABRIK : public SkeletonModification3D {
void set_fabrik_joint_use_target_basis(int p_joint_idx, bool p_use_basis);
real_t get_fabrik_joint_roll(int p_joint_idx) const;
void set_fabrik_joint_roll(int p_joint_idx, real_t p_roll);
real_t get_fabrik_joint_rotational_constraint(int p_joint_idx) const;
void set_fabrik_joint_rotational_constraint(int p_joint_idx, real_t p_rot);

SkeletonModification3DFABRIK();
~SkeletonModification3DFABRIK();
Expand Down