diff options
Diffstat (limited to 'scene/3d/skeleton_ik_3d.cpp')
| -rw-r--r-- | scene/3d/skeleton_ik_3d.cpp | 12 |
1 files changed, 4 insertions, 8 deletions
diff --git a/scene/3d/skeleton_ik_3d.cpp b/scene/3d/skeleton_ik_3d.cpp index 8d90aabfac..2e788051f4 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 |