Skip to content

Commit

Permalink
Fix SkeletonModification2DTwoBoneIK with negative scales.
Browse files Browse the repository at this point in the history
- Identifies when only one scale axis is negative and change the logic
- Take in count scales when drawing gizmo
- Fix typo
  • Loading branch information
thiagola92 committed Sep 19, 2023
1 parent 571cd0e commit 3e02983
Showing 1 changed file with 24 additions and 7 deletions.
31 changes: 24 additions & 7 deletions scene/resources/skeleton_modification_2d_twoboneik.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -138,17 +138,18 @@ void SkeletonModification2DTwoBoneIK::_execute(float p_delta) {
return;
}

// Adopted from the links below:
// Adapted from the links below:
// http://theorangeduck.com/page/simple-two-joint
// https://www.alanzucconi.com/2018/05/02/ik-2d-2/
// With modifications by TwistedTwigleg
Vector2 target_difference = target->get_global_position() - joint_one_bone->get_global_position();
float joint_one_to_target = target_difference.length();
float angle_atan = target_difference.angle();

float bone_one_length = joint_one_bone->get_length() * MIN(joint_one_bone->get_global_scale().x, joint_one_bone->get_global_scale().y);
float bone_two_length = joint_two_bone->get_length() * MIN(joint_two_bone->get_global_scale().x, joint_two_bone->get_global_scale().y);
float bone_one_length = joint_one_bone->get_length() * MIN(joint_one_bone->get_global_scale().abs().x, joint_one_bone->get_global_scale().abs().y);
float bone_two_length = joint_two_bone->get_length() * MIN(joint_two_bone->get_global_scale().abs().x, joint_two_bone->get_global_scale().abs().y);
bool override_angles_due_to_out_of_range = false;
bool same_scale_sign = true;

if (joint_one_to_target < target_minimum_distance) {
joint_one_to_target = target_minimum_distance;
Expand All @@ -161,6 +162,10 @@ void SkeletonModification2DTwoBoneIK::_execute(float p_delta) {
override_angles_due_to_out_of_range = true;
}

if (joint_one_bone->get_global_scale().sign().x != joint_one_bone->get_global_scale().sign().y) {
same_scale_sign = false;
}

if (!override_angles_due_to_out_of_range) {
float angle_0 = Math::acos(((joint_one_to_target * joint_one_to_target) + (bone_one_length * bone_one_length) - (bone_two_length * bone_two_length)) / (2.0 * joint_one_to_target * bone_one_length));
float angle_1 = Math::acos(((bone_two_length * bone_two_length) + (bone_one_length * bone_one_length) - (joint_one_to_target * joint_one_to_target)) / (2.0 * bone_two_length * bone_one_length));
Expand All @@ -173,12 +178,23 @@ void SkeletonModification2DTwoBoneIK::_execute(float p_delta) {
if (isnan(angle_0) || isnan(angle_1)) {
// We cannot solve for this angle! Do nothing to avoid setting the rotation (and scale) to NaN.
} else {
joint_one_bone->set_global_rotation(angle_atan - angle_0 - joint_one_bone->get_bone_angle());
if (same_scale_sign) {
joint_one_bone->set_global_rotation(angle_atan - angle_0 - joint_one_bone->get_bone_angle());
} else {
joint_one_bone->set_global_rotation(angle_atan + angle_0 + joint_one_bone->get_bone_angle());
}

joint_two_bone->set_rotation(-Math_PI - angle_1 - joint_two_bone->get_bone_angle() + joint_one_bone->get_bone_angle());
}

} else {
joint_one_bone->set_global_rotation(angle_atan - joint_one_bone->get_bone_angle());
joint_two_bone->set_global_rotation(angle_atan - joint_two_bone->get_bone_angle());
if (same_scale_sign) {
joint_one_bone->set_global_rotation(angle_atan - joint_one_bone->get_bone_angle());
joint_two_bone->set_global_rotation(angle_atan - joint_two_bone->get_bone_angle());
} else {
joint_one_bone->set_global_rotation(angle_atan + joint_one_bone->get_bone_angle());
joint_two_bone->set_global_rotation(angle_atan + joint_two_bone->get_bone_angle());
}
}

stack->skeleton->set_bone_local_pose_override(joint_one_bone_idx, joint_one_bone->get_transform(), stack->strength, true);
Expand Down Expand Up @@ -207,7 +223,8 @@ void SkeletonModification2DTwoBoneIK::_draw_editor_gizmo() {
}
stack->skeleton->draw_set_transform(
stack->skeleton->to_local(operation_bone_one->get_global_position()),
operation_bone_one->get_global_rotation() - stack->skeleton->get_global_rotation());
operation_bone_one->get_global_rotation() - stack->skeleton->get_global_rotation(),
operation_bone_one->get_global_scale());

Color bone_ik_color = Color(1.0, 0.65, 0.0, 0.4);
#ifdef TOOLS_ENABLED
Expand Down

0 comments on commit 3e02983

Please sign in to comment.