From a51e78528f20c410880b71c978f4038532d99659 Mon Sep 17 00:00:00 2001
From: Andrea Catania <info@andreacatania.com>
Date: Tue, 24 Mar 2020 09:59:32 +0100
Subject: [PATCH] Fixed IK rotation issue

(cherry picked from commit 277696d6c50eba68ca11cf3c0988be0213e5c65d)
---
 editor/plugins/skeleton_ik_editor_plugin.cpp | 5 +----
 scene/3d/skeleton.cpp                        | 8 ++++++++
 scene/3d/skeleton.h                          | 1 +
 scene/animation/skeleton_ik.cpp              | 7 +++++--
 4 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/editor/plugins/skeleton_ik_editor_plugin.cpp b/editor/plugins/skeleton_ik_editor_plugin.cpp
index eb6ad9498dd..41984d19beb 100644
--- a/editor/plugins/skeleton_ik_editor_plugin.cpp
+++ b/editor/plugins/skeleton_ik_editor_plugin.cpp
@@ -44,10 +44,7 @@ void SkeletonIKEditorPlugin::_play() {
 		skeleton_ik->start();
 	} else {
 		skeleton_ik->stop();
-
-		for (int i = 0; i < skeleton_ik->get_parent_skeleton()->get_bone_count(); ++i) {
-			skeleton_ik->get_parent_skeleton()->set_bone_global_pose_override(i, Transform(), 0);
-		}
+		skeleton_ik->get_parent_skeleton()->clear_bones_global_pose_override();
 	}
 }
 
diff --git a/scene/3d/skeleton.cpp b/scene/3d/skeleton.cpp
index be9bd93e621..8a47d9e8716 100644
--- a/scene/3d/skeleton.cpp
+++ b/scene/3d/skeleton.cpp
@@ -375,6 +375,13 @@ void Skeleton::_notification(int p_what) {
 	}
 }
 
+void Skeleton::clear_bones_global_pose_override() {
+	for (int i = 0; i < bones.size(); i += 1) {
+		bones.write[i].global_pose_override_amount = 0;
+	}
+	_make_dirty();
+}
+
 void Skeleton::set_bone_global_pose_override(int p_bone, const Transform &p_pose, float p_amount, bool p_persistent) {
 
 	ERR_FAIL_INDEX(p_bone, bones.size());
@@ -869,6 +876,7 @@ void Skeleton::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_bone_pose", "bone_idx"), &Skeleton::get_bone_pose);
 	ClassDB::bind_method(D_METHOD("set_bone_pose", "bone_idx", "pose"), &Skeleton::set_bone_pose);
 
+	ClassDB::bind_method(D_METHOD("clear_bones_global_pose_override"), &Skeleton::clear_bones_global_pose_override);
 	ClassDB::bind_method(D_METHOD("set_bone_global_pose_override", "bone_idx", "pose", "amount", "persistent"), &Skeleton::set_bone_global_pose_override, DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("get_bone_global_pose", "bone_idx"), &Skeleton::get_bone_global_pose);
 
diff --git a/scene/3d/skeleton.h b/scene/3d/skeleton.h
index 6a69fbdb93f..74f9f76c475 100644
--- a/scene/3d/skeleton.h
+++ b/scene/3d/skeleton.h
@@ -177,6 +177,7 @@ public:
 	Transform get_bone_rest(int p_bone) const;
 	Transform get_bone_global_pose(int p_bone) const;
 
+	void clear_bones_global_pose_override();
 	void set_bone_global_pose_override(int p_bone, const Transform &p_pose, float p_amount, bool p_persistent = false);
 
 	void set_bone_enabled(int p_bone, bool p_enabled);
diff --git a/scene/animation/skeleton_ik.cpp b/scene/animation/skeleton_ik.cpp
index 518c243dd0e..409fc0acc59 100644
--- a/scene/animation/skeleton_ik.cpp
+++ b/scene/animation/skeleton_ik.cpp
@@ -144,8 +144,9 @@ void FabrikInverseKinematic::update_chain(const Skeleton *p_sk, ChainItem *p_cha
 	p_chain_item->initial_transform = p_sk->get_bone_global_pose(p_chain_item->bone);
 	p_chain_item->current_pos = p_chain_item->initial_transform.origin;
 
-	for (int i = p_chain_item->children.size() - 1; 0 <= i; --i) {
-		update_chain(p_sk, &p_chain_item->children.write[i]);
+	ChainItem *items = p_chain_item->children.ptrw();
+	for (int i = 0; i < p_chain_item->children.size(); i += 1) {
+		update_chain(p_sk, items + i);
 	}
 }
 
@@ -286,6 +287,8 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
 		return; // Skip solving
 	}
 
+	p_task->skeleton->clear_bones_global_pose_override();
+
 	make_goal(p_task, p_task->skeleton->get_global_transform().affine_inverse().scaled(p_task->skeleton->get_global_transform().get_basis().get_scale()), blending_delta);
 
 	update_chain(p_task->skeleton, &p_task->chain.chain_root);