From 3e029832dc11e2a150f58b1f6829585c348fe7d8 Mon Sep 17 00:00:00 2001 From: thiagola92 Date: Thu, 17 Aug 2023 11:28:59 -0300 Subject: [PATCH] Fix SkeletonModification2DTwoBoneIK with negative scales. - Identifies when only one scale axis is negative and change the logic - Take in count scales when drawing gizmo - Fix typo --- .../skeleton_modification_2d_twoboneik.cpp | 31 ++++++++++++++----- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/scene/resources/skeleton_modification_2d_twoboneik.cpp b/scene/resources/skeleton_modification_2d_twoboneik.cpp index 1e8eb9842caa..f28d91bf9ce6 100644 --- a/scene/resources/skeleton_modification_2d_twoboneik.cpp +++ b/scene/resources/skeleton_modification_2d_twoboneik.cpp @@ -138,7 +138,7 @@ 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 @@ -146,9 +146,10 @@ void SkeletonModification2DTwoBoneIK::_execute(float p_delta) { 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; @@ -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)); @@ -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); @@ -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