Changes to SkeletonIK:

* Removed the pointers to PhysicalBone in the code, as they were unused.
* Forward ported the SkeletonIK bone scaling fix I made from Godot 3.2 to Godot 4.0.
* Fixed issue where the root bone in the IK chain would not rotate correctly.
  * The issue turned out to be the update_chain function being called in solve. This would override the root bone transform incorrectly and that would cause it not to rotate after just a single solve. Removing the update_chain function fixes the issue and based on my testing there are no adverse effects.
  * While the old fix on this PR (prior to a force push) required a hack fix, this new fix does not!
* Removed the update_chain function. This change doesn't appear to have any adverse effects in any of the projects I tested (including with animations, Skeleton3D or otherwise, from AnimationPlayer nodes!)
* Fixed issue where the scale of the Skeleton node would change the position of the target, causing it not to work with skeletons that have a global scale of anything but 1.

1 files changed, 7 insertions, 21 deletions

diff --git a/scene/3d/skeleton_ik_3d.cpp b/scene/3d/skeleton_ik_3d.cpp
index 9023f3c68a..bb1e2cbd9f 100644
--- a/scene/3d/skeleton_ik_3d.cpp
+++ b/scene/3d/skeleton_ik_3d.cpp
@@ -63,7 +63,6 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 	chain.chain_root.bone = p_task->root_bone;
 	chain.chain_root.initial_transform = p_task->skeleton->get_bone_global_pose(chain.chain_root.bone);
 	chain.chain_root.current_pos = chain.chain_root.initial_transform.origin;
-	chain.chain_root.pb = p_task->skeleton->get_physical_bone(chain.chain_root.bone);
 	chain.middle_chain_item = nullptr;
 
 	// Holds all IDs that are composing a single chain in reverse order
@@ -96,8 +95,6 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 			if (!child_ci) {
 				child_ci = sub_chain->add_child(chain_ids[i]);
 
-				child_ci->pb = p_task->skeleton->get_physical_bone(child_ci->bone);
-
 				child_ci->initial_transform = p_task->skeleton->get_bone_global_pose(child_ci->bone);
 				child_ci->current_pos = child_ci->initial_transform.origin;
 
@@ -132,20 +129,6 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 	return true;
 }
 
-void FabrikInverseKinematic::update_chain(const Skeleton3D *p_sk, ChainItem *p_chain_item) {
-	if (!p_chain_item) {
-		return;
-	}
-
-	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;
-
-	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);
-	}
-}
-
 void FabrikInverseKinematic::solve_simple(Task *p_task, bool p_solve_magnet) {
 	real_t distance_to_goal(1e4);
 	real_t previous_distance_to_goal(0);
@@ -263,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.write[0].tip_bone));
+		const Transform end_effector_pose(p_task->skeleton->get_bone_global_pose(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);
@@ -285,9 +268,7 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
 		p_task->skeleton->set_bone_global_pose_override(p_task->chain.tips[i].chain_item->bone, Transform(), 0.0, true);
 	}
 
-	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);
+	make_goal(p_task, p_task->skeleton->get_global_transform().affine_inverse(), blending_delta);
 
 	if (p_use_magnet && p_task->chain.middle_chain_item) {
 		p_task->chain.magnet_position = p_task->chain.middle_chain_item->initial_transform.origin.lerp(p_magnet_position, blending_delta);
@@ -310,6 +291,7 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
 				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) {
@@ -319,6 +301,10 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
 			}
 		}
 
+		// IK should not affect scale, so undo any scaling
+		new_bone_pose.basis.orthonormalize();
+		new_bone_pose.basis.scale(p_task->skeleton->get_bone_global_pose(ci->bone).basis.get_scale());
+
 		p_task->skeleton->set_bone_global_pose_override(ci->bone, new_bone_pose, 1.0, true);
 
 		if (!ci->children.empty()) {