Merge pull request #53821 from TwistedTwigleg/Godot_Master_SkeletonModificationIK_FixPoseChange

Fix for SkeletonModification3Ds to work with the new bone pose changes.

6 files changed, 72 insertions, 45 deletions

diff --git a/scene/3d/skeleton_3d.cpp b/scene/3d/skeleton_3d.cpp
index 14506fd9bd..fbc3767151 100644
--- a/scene/3d/skeleton_3d.cpp
+++ b/scene/3d/skeleton_3d.cpp
@@ -1113,7 +1113,7 @@ Transform3D Skeleton3D::global_pose_to_local_pose(int p_bone_idx, Transform3D p_
 	ERR_FAIL_INDEX_V(p_bone_idx, bone_size, Transform3D());
 	if (bones[p_bone_idx].parent >= 0) {
 		int parent_bone_idx = bones[p_bone_idx].parent;
-		Transform3D conversion_transform = bones[parent_bone_idx].pose_global.affine_inverse();
+		Transform3D conversion_transform = get_bone_global_pose(parent_bone_idx).affine_inverse();
 		return conversion_transform * p_global_pose;
 	} else {
 		return p_global_pose;
diff --git a/scene/resources/skeleton_modification_3d_fabrik.cpp b/scene/resources/skeleton_modification_3d_fabrik.cpp
index 20ebbda256..dedea3e282 100644
--- a/scene/resources/skeleton_modification_3d_fabrik.cpp
+++ b/scene/resources/skeleton_modification_3d_fabrik.cpp
@@ -149,6 +149,11 @@ void SkeletonModification3DFABRIK::_execute(real_t p_delta) {
 		return;
 	}
 
+	// Make sure the transform cache is the correct size
+	if (fabrik_transforms.size() != fabrik_data_chain.size()) {
+		fabrik_transforms.resize(fabrik_data_chain.size());
+	}
+
 	// Verify that all joints have a valid bone ID, and that all bone lengths are zero or more
 	// Also, while we are here, apply magnet positions.
 	for (uint32_t i = 0; i < fabrik_data_chain.size(); i++) {
@@ -162,27 +167,24 @@ void SkeletonModification3DFABRIK::_execute(real_t p_delta) {
 		if (_print_execution_error(fabrik_data_chain[i].length < 0, "FABRIK Joint " + itos(i) + " has an invalid joint length. Cannot execute!")) {
 			return;
 		}
-
-		Transform3D local_pose_override = stack->skeleton->get_bone_local_pose_override(fabrik_data_chain[i].bone_idx);
+		fabrik_transforms[i] = stack->skeleton->get_bone_global_pose(fabrik_data_chain[i].bone_idx);
 
 		// Apply magnet positions:
 		if (stack->skeleton->get_bone_parent(fabrik_data_chain[i].bone_idx) >= 0) {
 			int parent_bone_idx = stack->skeleton->get_bone_parent(fabrik_data_chain[i].bone_idx);
 			Transform3D conversion_transform = (stack->skeleton->get_bone_global_pose(parent_bone_idx));
-			local_pose_override.origin += conversion_transform.basis.xform_inv(fabrik_data_chain[i].magnet_position);
+			fabrik_transforms[i].origin += conversion_transform.basis.xform_inv(fabrik_data_chain[i].magnet_position);
 		} else {
-			local_pose_override.origin += fabrik_data_chain[i].magnet_position;
+			fabrik_transforms[i].origin += fabrik_data_chain[i].magnet_position;
 		}
-
-		stack->skeleton->set_bone_local_pose_override(fabrik_data_chain[i].bone_idx, local_pose_override, stack->strength, true);
 	}
+	Transform3D origin_global_pose_trans = stack->skeleton->get_bone_global_pose_no_override(fabrik_data_chain[0].bone_idx);
 
 	target_global_pose = stack->skeleton->world_transform_to_global_pose(node_target->get_global_transform());
-	origin_global_pose = stack->skeleton->local_pose_to_global_pose(
-			fabrik_data_chain[0].bone_idx, stack->skeleton->get_bone_local_pose_override(fabrik_data_chain[0].bone_idx));
+	origin_global_pose = origin_global_pose_trans;
 
 	final_joint_idx = fabrik_data_chain.size() - 1;
-	real_t target_distance = stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[final_joint_idx].bone_idx, target_global_pose).origin.length();
+	real_t target_distance = fabrik_transforms[final_joint_idx].origin.distance_to(target_global_pose.origin);
 	chain_iterations = 0;
 
 	while (target_distance > chain_tolerance) {
@@ -190,7 +192,7 @@ void SkeletonModification3DFABRIK::_execute(real_t p_delta) {
 		chain_forwards();
 
 		// update the target distance
-		target_distance = stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[final_joint_idx].bone_idx, target_global_pose).origin.length();
+		target_distance = fabrik_transforms[final_joint_idx].origin.distance_to(target_global_pose.origin);
 
 		// update chain iterations
 		chain_iterations += 1;
@@ -205,7 +207,7 @@ void SkeletonModification3DFABRIK::_execute(real_t p_delta) {
 
 void SkeletonModification3DFABRIK::chain_backwards() {
 	int final_bone_idx = fabrik_data_chain[final_joint_idx].bone_idx;
-	Transform3D final_joint_trans = stack->skeleton->local_pose_to_global_pose(final_bone_idx, stack->skeleton->get_bone_local_pose_override(final_bone_idx));
+	Transform3D final_joint_trans = fabrik_transforms[final_joint_idx];
 
 	// Get the direction the final bone is facing in.
 	stack->skeleton->update_bone_rest_forward_vector(final_bone_idx);
@@ -220,52 +222,46 @@ void SkeletonModification3DFABRIK::chain_backwards() {
 
 	// set the position of the final joint to the target position
 	final_joint_trans.origin = target_global_pose.origin - (direction * fabrik_data_chain[final_joint_idx].length);
-	final_joint_trans = stack->skeleton->global_pose_to_local_pose(final_bone_idx, final_joint_trans);
-	stack->skeleton->set_bone_local_pose_override(final_bone_idx, final_joint_trans, stack->strength, true);
+	fabrik_transforms[final_joint_idx] = final_joint_trans;
 
 	// for all other joints, move them towards the target
 	int i = final_joint_idx;
 	while (i >= 1) {
-		int next_bone_idx = fabrik_data_chain[i].bone_idx;
-		Transform3D next_bone_trans = stack->skeleton->local_pose_to_global_pose(next_bone_idx, stack->skeleton->get_bone_local_pose_override(next_bone_idx));
+		Transform3D next_bone_trans = fabrik_transforms[i];
 		i -= 1;
-		int current_bone_idx = fabrik_data_chain[i].bone_idx;
-		Transform3D current_trans = stack->skeleton->local_pose_to_global_pose(current_bone_idx, stack->skeleton->get_bone_local_pose_override(current_bone_idx));
+		Transform3D current_trans = fabrik_transforms[i];
 
 		real_t length = fabrik_data_chain[i].length / (current_trans.origin.distance_to(next_bone_trans.origin));
 		current_trans.origin = next_bone_trans.origin.lerp(current_trans.origin, length);
 
-		// Apply it back to the skeleton
-		stack->skeleton->set_bone_local_pose_override(current_bone_idx, stack->skeleton->global_pose_to_local_pose(current_bone_idx, current_trans), stack->strength, true);
+		// Save the result
+		fabrik_transforms[i] = current_trans;
 	}
 }
 
 void SkeletonModification3DFABRIK::chain_forwards() {
 	// Set root at the initial position.
-	int origin_bone_idx = fabrik_data_chain[0].bone_idx;
-	Transform3D root_transform = stack->skeleton->local_pose_to_global_pose(origin_bone_idx, stack->skeleton->get_bone_local_pose_override(origin_bone_idx));
+	Transform3D root_transform = fabrik_transforms[0];
+
 	root_transform.origin = origin_global_pose.origin;
-	stack->skeleton->set_bone_local_pose_override(origin_bone_idx, stack->skeleton->global_pose_to_local_pose(origin_bone_idx, root_transform), stack->strength, true);
+	fabrik_transforms[0] = origin_global_pose;
 
 	for (uint32_t i = 0; i < fabrik_data_chain.size() - 1; i++) {
-		int current_bone_idx = fabrik_data_chain[i].bone_idx;
-		Transform3D current_trans = stack->skeleton->local_pose_to_global_pose(current_bone_idx, stack->skeleton->get_bone_local_pose_override(current_bone_idx));
-		int next_bone_idx = fabrik_data_chain[i + 1].bone_idx;
-		Transform3D next_bone_trans = stack->skeleton->local_pose_to_global_pose(next_bone_idx, stack->skeleton->get_bone_local_pose_override(next_bone_idx));
+		Transform3D current_trans = fabrik_transforms[i];
+		Transform3D next_bone_trans = fabrik_transforms[i + 1];
 
 		real_t length = fabrik_data_chain[i].length / (next_bone_trans.origin.distance_to(current_trans.origin));
 		next_bone_trans.origin = current_trans.origin.lerp(next_bone_trans.origin, length);
 
-		// Apply it back to the skeleton
-		stack->skeleton->set_bone_local_pose_override(next_bone_idx, stack->skeleton->global_pose_to_local_pose(next_bone_idx, next_bone_trans), stack->strength, true);
+		// Save the result
+		fabrik_transforms[i + 1] = next_bone_trans;
 	}
 }
 
 void SkeletonModification3DFABRIK::chain_apply() {
 	for (uint32_t i = 0; i < fabrik_data_chain.size(); i++) {
 		int current_bone_idx = fabrik_data_chain[i].bone_idx;
-		Transform3D current_trans = stack->skeleton->get_bone_local_pose_override(current_bone_idx);
-		current_trans = stack->skeleton->local_pose_to_global_pose(current_bone_idx, current_trans);
+		Transform3D current_trans = fabrik_transforms[i];
 
 		// If this is the last bone in the chain...
 		if (i == fabrik_data_chain.size() - 1) {
@@ -280,8 +276,7 @@ void SkeletonModification3DFABRIK::chain_apply() {
 				current_trans.basis = target_global_pose.basis.orthonormalized().scaled(current_trans.basis.get_scale());
 			}
 		} else { // every other bone in the chain...
-			int next_bone_idx = fabrik_data_chain[i + 1].bone_idx;
-			Transform3D next_trans = stack->skeleton->local_pose_to_global_pose(next_bone_idx, stack->skeleton->get_bone_local_pose_override(next_bone_idx));
+			Transform3D next_trans = fabrik_transforms[i + 1];
 
 			// Get the forward direction that the basis is facing in right now.
 			stack->skeleton->update_bone_rest_forward_vector(current_bone_idx);
@@ -290,9 +285,7 @@ void SkeletonModification3DFABRIK::chain_apply() {
 			current_trans.basis.rotate_to_align(forward_vector, current_trans.origin.direction_to(next_trans.origin));
 			current_trans.basis.rotate_local(forward_vector, fabrik_data_chain[i].roll);
 		}
-		current_trans = stack->skeleton->global_pose_to_local_pose(current_bone_idx, current_trans);
-		current_trans.origin = Vector3(0, 0, 0);
-		stack->skeleton->set_bone_local_pose_override(current_bone_idx, current_trans, stack->strength, true);
+		stack->skeleton->set_bone_local_pose_override(current_bone_idx, stack->skeleton->global_pose_to_local_pose(current_bone_idx, current_trans), stack->strength, true);
 	}
 
 	// Update all the bones so the next modification has up-to-date data.
@@ -374,6 +367,7 @@ int SkeletonModification3DFABRIK::get_fabrik_data_chain_length() {
 void SkeletonModification3DFABRIK::set_fabrik_data_chain_length(int p_length) {
 	ERR_FAIL_COND(p_length < 0);
 	fabrik_data_chain.resize(p_length);
+	fabrik_transforms.resize(p_length);
 	execution_error_found = false;
 	notify_property_list_changed();
 }
@@ -513,8 +507,11 @@ void SkeletonModification3DFABRIK::fabrik_joint_auto_calculate_length(int p_join
 
 		Transform3D node_trans = tip_node->get_global_transform();
 		node_trans = stack->skeleton->world_transform_to_global_pose(node_trans);
-		node_trans = stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[p_joint_idx].bone_idx, node_trans);
-		fabrik_data_chain[p_joint_idx].length = node_trans.origin.length();
+		//node_trans = stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[p_joint_idx].bone_idx, node_trans);
+		//fabrik_data_chain[p_joint_idx].length = node_trans.origin.length();
+
+		fabrik_data_chain[p_joint_idx].length = stack->skeleton->get_bone_global_pose(fabrik_data_chain[p_joint_idx].bone_idx).origin.distance_to(node_trans.origin);
+
 	} else { // Use child bone(s) to update joint length, if possible
 		Vector<int> bone_children = stack->skeleton->get_bone_children(fabrik_data_chain[p_joint_idx].bone_idx);
 		if (bone_children.size() <= 0) {
@@ -522,10 +519,13 @@ void SkeletonModification3DFABRIK::fabrik_joint_auto_calculate_length(int p_join
 			return;
 		}
 
+		Transform3D bone_trans = stack->skeleton->get_bone_global_pose(fabrik_data_chain[p_joint_idx].bone_idx);
+
 		real_t final_length = 0;
 		for (int i = 0; i < bone_children.size(); i++) {
 			Transform3D child_transform = stack->skeleton->get_bone_global_pose(bone_children[i]);
-			final_length += stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[p_joint_idx].bone_idx, child_transform).origin.length();
+			final_length += bone_trans.origin.distance_to(child_transform.origin);
+			//final_length += stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[p_joint_idx].bone_idx, child_transform).origin.length();
 		}
 		fabrik_data_chain[p_joint_idx].length = final_length / bone_children.size();
 	}
diff --git a/scene/resources/skeleton_modification_3d_fabrik.h b/scene/resources/skeleton_modification_3d_fabrik.h
index 9b5da883d4..6c58b8a07a 100644
--- a/scene/resources/skeleton_modification_3d_fabrik.h
+++ b/scene/resources/skeleton_modification_3d_fabrik.h
@@ -55,6 +55,8 @@ private:
 	};
 
 	LocalVector<FABRIK_Joint_Data> fabrik_data_chain;
+	LocalVector<Transform3D> fabrik_transforms;
+
 	NodePath target_node;
 	ObjectID target_node_cache;
 
diff --git a/scene/resources/skeleton_modification_3d_jiggle.cpp b/scene/resources/skeleton_modification_3d_jiggle.cpp
index 1fb7dad2ad..a6bcb0176a 100644
--- a/scene/resources/skeleton_modification_3d_jiggle.cpp
+++ b/scene/resources/skeleton_modification_3d_jiggle.cpp
@@ -172,7 +172,12 @@ void SkeletonModification3DJiggle::_execute_jiggle_joint(int p_joint_idx, Node3D
 		return;
 	}
 
-	Transform3D new_bone_trans = stack->skeleton->local_pose_to_global_pose(jiggle_data_chain[p_joint_idx].bone_idx, stack->skeleton->get_bone_local_pose_override(jiggle_data_chain[p_joint_idx].bone_idx));
+	Transform3D bone_local_pos = stack->skeleton->get_bone_local_pose_override(jiggle_data_chain[p_joint_idx].bone_idx);
+	if (bone_local_pos == Transform3D()) {
+		bone_local_pos = stack->skeleton->get_bone_pose(jiggle_data_chain[p_joint_idx].bone_idx);
+	}
+
+	Transform3D new_bone_trans = stack->skeleton->local_pose_to_global_pose(jiggle_data_chain[p_joint_idx].bone_idx, bone_local_pos);
 	Vector3 target_position = stack->skeleton->world_transform_to_global_pose(p_target->get_global_transform()).origin;
 
 	jiggle_data_chain[p_joint_idx].force = (target_position - jiggle_data_chain[p_joint_idx].dynamic_position) * jiggle_data_chain[p_joint_idx].stiffness * p_delta;
diff --git a/scene/resources/skeleton_modification_3d_lookat.cpp b/scene/resources/skeleton_modification_3d_lookat.cpp
index afdb077e71..f3b0f41d60 100644
--- a/scene/resources/skeleton_modification_3d_lookat.cpp
+++ b/scene/resources/skeleton_modification_3d_lookat.cpp
@@ -96,8 +96,10 @@ void SkeletonModification3DLookAt::_execute(real_t p_delta) {
 	if (_print_execution_error(bone_idx <= -1, "Bone index is invalid. Cannot execute modification!")) {
 		return;
 	}
-
 	Transform3D new_bone_trans = stack->skeleton->get_bone_local_pose_override(bone_idx);
+	if (new_bone_trans == Transform3D()) {
+		new_bone_trans = stack->skeleton->get_bone_pose(bone_idx);
+	}
 	Vector3 target_pos = stack->skeleton->global_pose_to_local_pose(bone_idx, stack->skeleton->world_transform_to_global_pose(target->get_global_transform())).origin;
 
 	// Lock the rotation to a plane relative to the bone by changing the target position
diff --git a/scene/resources/skeleton_modification_3d_twoboneik.cpp b/scene/resources/skeleton_modification_3d_twoboneik.cpp
index c1a71148a7..93ec155a88 100644
--- a/scene/resources/skeleton_modification_3d_twoboneik.cpp
+++ b/scene/resources/skeleton_modification_3d_twoboneik.cpp
@@ -178,7 +178,16 @@ void SkeletonModification3DTwoBoneIK::_execute(real_t p_delta) {
 		}
 		Transform3D pole_trans = stack->skeleton->world_transform_to_global_pose(pole->get_global_transform());
 
-		bone_one_trans = stack->skeleton->local_pose_to_global_pose(joint_one_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_one_bone_idx));
+		Transform3D bone_one_local_pos = stack->skeleton->get_bone_local_pose_override(joint_one_bone_idx);
+		if (bone_one_local_pos == Transform3D()) {
+			bone_one_local_pos = stack->skeleton->get_bone_pose(joint_one_bone_idx);
+		}
+		Transform3D bone_two_local_pos = stack->skeleton->get_bone_local_pose_override(joint_two_bone_idx);
+		if (bone_two_local_pos == Transform3D()) {
+			bone_two_local_pos = stack->skeleton->get_bone_pose(joint_two_bone_idx);
+		}
+
+		bone_one_trans = stack->skeleton->local_pose_to_global_pose(joint_one_bone_idx, bone_one_local_pos);
 		bone_one_trans = bone_one_trans.looking_at(pole_trans.origin, Vector3(0, 1, 0));
 		bone_one_trans.basis = stack->skeleton->global_pose_z_forward_to_bone_forward(joint_one_bone_idx, bone_one_trans.basis);
 		stack->skeleton->update_bone_rest_forward_vector(joint_one_bone_idx);
@@ -186,7 +195,7 @@ void SkeletonModification3DTwoBoneIK::_execute(real_t p_delta) {
 		stack->skeleton->set_bone_local_pose_override(joint_one_bone_idx, stack->skeleton->global_pose_to_local_pose(joint_one_bone_idx, bone_one_trans), stack->strength, true);
 		stack->skeleton->force_update_bone_children_transforms(joint_one_bone_idx);
 
-		bone_two_trans = stack->skeleton->local_pose_to_global_pose(joint_two_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_two_bone_idx));
+		bone_two_trans = stack->skeleton->local_pose_to_global_pose(joint_two_bone_idx, bone_two_local_pos);
 		bone_two_trans = bone_two_trans.looking_at(target_trans.origin, Vector3(0, 1, 0));
 		bone_two_trans.basis = stack->skeleton->global_pose_z_forward_to_bone_forward(joint_two_bone_idx, bone_two_trans.basis);
 		stack->skeleton->update_bone_rest_forward_vector(joint_two_bone_idx);
@@ -194,8 +203,17 @@ void SkeletonModification3DTwoBoneIK::_execute(real_t p_delta) {
 		stack->skeleton->set_bone_local_pose_override(joint_two_bone_idx, stack->skeleton->global_pose_to_local_pose(joint_two_bone_idx, bone_two_trans), stack->strength, true);
 		stack->skeleton->force_update_bone_children_transforms(joint_two_bone_idx);
 	} else {
-		bone_one_trans = stack->skeleton->local_pose_to_global_pose(joint_one_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_one_bone_idx));
-		bone_two_trans = stack->skeleton->local_pose_to_global_pose(joint_two_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_two_bone_idx));
+		Transform3D bone_one_local_pos = stack->skeleton->get_bone_local_pose_override(joint_one_bone_idx);
+		if (bone_one_local_pos == Transform3D()) {
+			bone_one_local_pos = stack->skeleton->get_bone_pose(joint_one_bone_idx);
+		}
+		Transform3D bone_two_local_pos = stack->skeleton->get_bone_local_pose_override(joint_two_bone_idx);
+		if (bone_two_local_pos == Transform3D()) {
+			bone_two_local_pos = stack->skeleton->get_bone_pose(joint_two_bone_idx);
+		}
+
+		bone_one_trans = stack->skeleton->local_pose_to_global_pose(joint_one_bone_idx, bone_one_local_pos);
+		bone_two_trans = stack->skeleton->local_pose_to_global_pose(joint_two_bone_idx, bone_two_local_pos);
 	}
 
 	Transform3D bone_two_tip_trans;