From 92a79ace5b3e58933ecdb0e21e3c3402073979b0 Mon Sep 17 00:00:00 2001 From: TwistedTwigleg Date: Mon, 4 Oct 2021 10:22:51 -0400 Subject: [PATCH] Fixed SkeletonIK not rotating correctly when solving --- scene/3d/skeleton_ik_3d.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/scene/3d/skeleton_ik_3d.cpp b/scene/3d/skeleton_ik_3d.cpp index 8d90aabfac6..2e788051f43 100644 --- a/scene/3d/skeleton_ik_3d.cpp +++ b/scene/3d/skeleton_ik_3d.cpp @@ -291,14 +291,10 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove new_bone_pose.origin = ci->current_pos; if (!ci->children.is_empty()) { - /// Rotate basis - const Vector3 initial_ori((ci->children[0].initial_transform.origin - ci->initial_transform.origin).normalized()); - const Vector3 rot_axis(initial_ori.cross(ci->current_ori).normalized()); - - if (rot_axis[0] != 0 && rot_axis[1] != 0 && rot_axis[2] != 0) { - const real_t rot_angle(Math::acos(CLAMP(initial_ori.dot(ci->current_ori), -1, 1))); - new_bone_pose.basis.rotate(rot_axis, rot_angle); - } + p_task->skeleton->update_bone_rest_forward_vector(ci->bone); + Vector3 forward_vector = p_task->skeleton->get_bone_axis_forward_vector(ci->bone); + // Rotate the bone towards the next bone in the chain: + new_bone_pose.basis.rotate_to_align(forward_vector, new_bone_pose.origin.direction_to(ci->children[0].current_pos)); } else { // Set target orientation to tip