diff --git a/doc/classes/SkeletonModification3DFABRIK.xml b/doc/classes/SkeletonModification3DFABRIK.xml
index 41f78fab4143..7d49d1b28ae6 100644
--- a/doc/classes/SkeletonModification3DFABRIK.xml
+++ b/doc/classes/SkeletonModification3DFABRIK.xml
@@ -55,6 +55,12 @@
Returns the magnet vector of the FABRIK joint at [code]joint_idx[/code].
+
+
+
+
+
+
@@ -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.
+
+
+
+
+
+
+
@@ -154,6 +167,8 @@
The amount of FABRIK joints in the FABRIK modification.
+
+
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.
diff --git a/scene/resources/skeleton_modification_3d_fabrik.cpp b/scene/resources/skeleton_modification_3d_fabrik.cpp
index b62dda3f4fa4..f2887c7bf4ca 100644
--- a/scene/resources/skeleton_modification_3d_fabrik.cpp
+++ b/scene/resources/skeleton_modification_3d_fabrik.cpp
@@ -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;
}
@@ -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;
}
@@ -105,6 +109,7 @@ void SkeletonModification3DFABRIK::_get_property_list(List *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) {
@@ -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];
@@ -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;
@@ -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;
}
@@ -583,6 +646,19 @@ 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 {
+ const int bone_chain_size = fabrik_data_chain.size();
+ ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, 0.0);
+ 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);
@@ -592,6 +668,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);
@@ -611,10 +689,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");
}
diff --git a/scene/resources/skeleton_modification_3d_fabrik.h b/scene/resources/skeleton_modification_3d_fabrik.h
index cc4d3a5e2012..96679f3fd517 100644
--- a/scene/resources/skeleton_modification_3d_fabrik.h
+++ b/scene/resources/skeleton_modification_3d_fabrik.h
@@ -52,6 +52,8 @@ class SkeletonModification3DFABRIK : public SkeletonModification3D {
bool use_target_basis = false;
real_t roll = 0;
+
+ real_t rotational_constraint = 0;
};
LocalVector fabrik_data_chain;
@@ -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);
@@ -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;
@@ -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;
@@ -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();