Fixes the SkeletonIK twisting issue by using the skeleton global pose without overrides

4 files changed, 77 insertions, 90 deletions

diff --git a/doc/classes/Skeleton3D.xml b/doc/classes/Skeleton3D.xml
index c6dd6fb142..0b278d7d25 100644
--- a/doc/classes/Skeleton3D.xml
+++ b/doc/classes/Skeleton3D.xml
@@ -91,6 +91,15 @@
 				Returns the overall transform of the specified bone, with respect to the skeleton. Being relative to the skeleton frame, this is not the actual "global" transform of the bone.
 			</description>
 		</method>
+		<method name="get_bone_global_pose_no_override" qualifiers="const">
+			<return type="Transform">
+			</return>
+			<argument index="0" name="bone_idx" type="int">
+			</argument>
+			<description>
+				Returns the overall transform of the specified bone, with respect to the skeleton, but without any global pose overrides. Being relative to the skeleton frame, this is not the actual "global" transform of the bone.
+			</description>
+		</method>
 		<method name="get_bone_name" qualifiers="const">
 			<return type="String">
 			</return>
diff --git a/scene/3d/skeleton_3d.cpp b/scene/3d/skeleton_3d.cpp
index ebbb8985c9..59233708f6 100644
--- a/scene/3d/skeleton_3d.cpp
+++ b/scene/3d/skeleton_3d.cpp
@@ -237,53 +237,57 @@ void Skeleton3D::_notification(int p_what) {
 			for (int i = 0; i < len; i++) {
 				Bone &b = bonesptr[order[i]];
 
-				if (b.global_pose_override_amount >= 0.999) {
-					b.pose_global = b.global_pose_override;
-				} else {
-					if (b.disable_rest) {
-						if (b.enabled) {
-							Transform pose = b.pose;
-							if (b.custom_pose_enable) {
-								pose = b.custom_pose * pose;
-							}
-							if (b.parent >= 0) {
-								b.pose_global = bonesptr[b.parent].pose_global * pose;
-							} else {
-								b.pose_global = pose;
-							}
+				if (b.disable_rest) {
+					if (b.enabled) {
+						Transform pose = b.pose;
+						if (b.custom_pose_enable) {
+							pose = b.custom_pose * pose;
+						}
+						if (b.parent >= 0) {
+							b.pose_global = bonesptr[b.parent].pose_global * pose;
+							b.pose_global_no_override = bonesptr[b.parent].pose_global * pose;
 						} else {
-							if (b.parent >= 0) {
-								b.pose_global = bonesptr[b.parent].pose_global;
-							} else {
-								b.pose_global = Transform();
-							}
+							b.pose_global = pose;
+							b.pose_global_no_override = pose;
 						}
-
 					} else {
-						if (b.enabled) {
-							Transform pose = b.pose;
-							if (b.custom_pose_enable) {
-								pose = b.custom_pose * pose;
-							}
-							if (b.parent >= 0) {
-								b.pose_global = bonesptr[b.parent].pose_global * (b.rest * pose);
-							} else {
-								b.pose_global = b.rest * pose;
-							}
+						if (b.parent >= 0) {
+							b.pose_global = bonesptr[b.parent].pose_global;
+							b.pose_global_no_override = bonesptr[b.parent].pose_global;
 						} else {
-							if (b.parent >= 0) {
-								b.pose_global = bonesptr[b.parent].pose_global * b.rest;
-							} else {
-								b.pose_global = b.rest;
-							}
+							b.pose_global = Transform();
+							b.pose_global_no_override = Transform();
 						}
 					}
 
-					if (b.global_pose_override_amount >= CMP_EPSILON) {
-						b.pose_global = b.pose_global.interpolate_with(b.global_pose_override, b.global_pose_override_amount);
+				} else {
+					if (b.enabled) {
+						Transform pose = b.pose;
+						if (b.custom_pose_enable) {
+							pose = b.custom_pose * pose;
+						}
+						if (b.parent >= 0) {
+							b.pose_global = bonesptr[b.parent].pose_global * (b.rest * pose);
+							b.pose_global_no_override = bonesptr[b.parent].pose_global * (b.rest * pose);
+						} else {
+							b.pose_global = b.rest * pose;
+							b.pose_global_no_override = b.rest * pose;
+						}
+					} else {
+						if (b.parent >= 0) {
+							b.pose_global = bonesptr[b.parent].pose_global * b.rest;
+							b.pose_global_no_override = bonesptr[b.parent].pose_global * b.rest;
+						} else {
+							b.pose_global = b.rest;
+							b.pose_global_no_override = b.rest;
+						}
 					}
 				}
 
+				if (b.global_pose_override_amount >= CMP_EPSILON) {
+					b.pose_global = b.pose_global.interpolate_with(b.global_pose_override, b.global_pose_override_amount);
+				}
+
 				if (b.global_pose_override_reset) {
 					b.global_pose_override_amount = 0.0;
 				}
@@ -408,6 +412,14 @@ Transform Skeleton3D::get_bone_global_pose(int p_bone) const {
 	return bones[p_bone].pose_global;
 }
 
+Transform Skeleton3D::get_bone_global_pose_no_override(int p_bone) const {
+	ERR_FAIL_INDEX_V(p_bone, bones.size(), Transform());
+	if (dirty) {
+		const_cast<Skeleton3D *>(this)->notification(NOTIFICATION_UPDATE_SKELETON);
+	}
+	return bones[p_bone].pose_global_no_override;
+}
+
 // skeleton creation api
 void Skeleton3D::add_bone(const String &p_name) {
 	ERR_FAIL_COND(p_name == "" || p_name.find(":") != -1 || p_name.find("/") != -1);
@@ -912,6 +924,7 @@ void Skeleton3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("clear_bones_global_pose_override"), &Skeleton3D::clear_bones_global_pose_override);
 	ClassDB::bind_method(D_METHOD("set_bone_global_pose_override", "bone_idx", "pose", "amount", "persistent"), &Skeleton3D::set_bone_global_pose_override, DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("get_bone_global_pose", "bone_idx"), &Skeleton3D::get_bone_global_pose);
+	ClassDB::bind_method(D_METHOD("get_bone_global_pose_no_override", "bone_idx"), &Skeleton3D::get_bone_global_pose_no_override);
 
 	ClassDB::bind_method(D_METHOD("get_bone_custom_pose", "bone_idx"), &Skeleton3D::get_bone_custom_pose);
 	ClassDB::bind_method(D_METHOD("set_bone_custom_pose", "bone_idx", "custom_pose"), &Skeleton3D::set_bone_custom_pose);
diff --git a/scene/3d/skeleton_3d.h b/scene/3d/skeleton_3d.h
index 2941ac2c45..508cd7c329 100644
--- a/scene/3d/skeleton_3d.h
+++ b/scene/3d/skeleton_3d.h
@@ -83,6 +83,7 @@ private:
 
 		Transform pose;
 		Transform pose_global;
+		Transform pose_global_no_override;
 
 		bool custom_pose_enable = false;
 		Transform custom_pose;
@@ -160,6 +161,7 @@ public:
 	void set_bone_rest(int p_bone, const Transform &p_rest);
 	Transform get_bone_rest(int p_bone) const;
 	Transform get_bone_global_pose(int p_bone) const;
+	Transform get_bone_global_pose_no_override(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);
diff --git a/scene/3d/skeleton_ik_3d.cpp b/scene/3d/skeleton_ik_3d.cpp
index 898f94ccc1..bd1c202205 100644
--- a/scene/3d/skeleton_ik_3d.cpp
+++ b/scene/3d/skeleton_ik_3d.cpp
@@ -246,7 +246,7 @@ void FabrikInverseKinematic::make_goal(Task *p_task, const Transform &p_inverse_
 		p_task->end_effectors.write[0].goal_transform = p_inverse_transf * p_task->goal_global_transform;
 	} else {
 		// End effector in local transform
-		const Transform end_effector_pose(p_task->skeleton->get_bone_global_pose(p_task->end_effectors[0].tip_bone));
+		const Transform end_effector_pose(p_task->skeleton->get_bone_global_pose_no_override(p_task->end_effectors[0].tip_bone));
 
 		// Update the end_effector (local transform) by blending with current pose
 		p_task->end_effectors.write[0].goal_transform = end_effector_pose.interpolate_with(p_inverse_transf * p_task->goal_global_transform, blending_delta);
@@ -270,18 +270,7 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
 		return; // Skip solving
 	}
 
-	p_task->skeleton->set_bone_global_pose_override(p_task->chain.chain_root.bone, Transform(), 0.0, true);
-
-	if (p_task->chain.middle_chain_item) {
-		p_task->skeleton->set_bone_global_pose_override(p_task->chain.middle_chain_item->bone, Transform(), 0.0, true);
-	}
-
-	for (int i = 0; i < p_task->chain.tips.size(); i += 1) {
-		p_task->skeleton->set_bone_global_pose_override(p_task->chain.tips[i].chain_item->bone, Transform(), 0.0, true);
-	}
-
-	// Update the transforms to their global poses
-	// (Needed to sync IK with animation)
+	// Update the initial root transform so its synced with any animation changes
 	_update_chain(p_task->skeleton, &p_task->chain.chain_root);
 
 	make_goal(p_task, p_task->skeleton->get_global_transform().affine_inverse(), blending_delta);
@@ -298,48 +287,22 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
 		Transform new_bone_pose(ci->initial_transform);
 		new_bone_pose.origin = ci->current_pos;
 
-		// The root bone needs to be rotated differently so it isn't frozen in place.
-		if (ci == &p_task->chain.chain_root && !ci->children.is_empty()) {
-			new_bone_pose = new_bone_pose.looking_at(ci->children[0].current_pos);
-			const Vector3 bone_rest_dir = p_task->skeleton->get_bone_rest(ci->children[0].bone).origin.normalized().abs();
-			const Vector3 bone_rest_dir_abs = bone_rest_dir.abs();
-			if (bone_rest_dir_abs.x > bone_rest_dir_abs.y && bone_rest_dir_abs.x > bone_rest_dir_abs.z) {
-				if (bone_rest_dir.x < 0) {
-					new_bone_pose.basis.rotate_local(Vector3(0, 1, 0), -Math_PI / 2.0f);
-				} else {
-					new_bone_pose.basis.rotate_local(Vector3(0, 1, 0), Math_PI / 2.0f);
-				}
-			} else if (bone_rest_dir_abs.y > bone_rest_dir_abs.x && bone_rest_dir_abs.y > bone_rest_dir_abs.z) {
-				if (bone_rest_dir.y < 0) {
-					new_bone_pose.basis.rotate_local(Vector3(1, 0, 0), Math_PI / 2.0f);
-				} else {
-					new_bone_pose.basis.rotate_local(Vector3(1, 0, 0), -Math_PI / 2.0f);
-				}
-			} else {
-				if (bone_rest_dir.z < 0) {
-					// Do nothing!
-				} else {
-					new_bone_pose.basis.rotate_local(Vector3(0, 0, 1), Math_PI);
-				}
-			}
-		} else {
-			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 (!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);
-				}
+			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);
+			}
 
+		} else {
+			// Set target orientation to tip
+			if (override_tip_basis) {
+				new_bone_pose.basis = p_task->chain.tips[0].end_effector->goal_transform.basis;
 			} else {
-				// Set target orientation to tip
-				if (override_tip_basis) {
-					new_bone_pose.basis = p_task->chain.tips[0].end_effector->goal_transform.basis;
-				} else {
-					new_bone_pose.basis = new_bone_pose.basis * p_task->chain.tips[0].end_effector->goal_transform.basis;
-				}
+				new_bone_pose.basis = new_bone_pose.basis * p_task->chain.tips[0].end_effector->goal_transform.basis;
 			}
 		}
 
@@ -362,7 +325,7 @@ void FabrikInverseKinematic::_update_chain(const Skeleton3D *p_sk, ChainItem *p_
 		return;
 	}
 
-	p_chain_item->initial_transform = p_sk->get_bone_global_pose(p_chain_item->bone);
+	p_chain_item->initial_transform = p_sk->get_bone_global_pose_no_override(p_chain_item->bone);
 	p_chain_item->current_pos = p_chain_item->initial_transform.origin;
 
 	ChainItem *items = p_chain_item->children.ptrw();