From 0cedc047697bd07d054f0aa7bc76f5dce22d41cd Mon Sep 17 00:00:00 2001 From: TwistedTwigleg Date: Thu, 14 Oct 2021 18:26:44 -0400 Subject: [PATCH] Fix for SkeletonModification3Ds to work with the new bone pose changes, fixed global_pose_to_local_pose function --- scene/3d/skeleton_3d.cpp | 2 +- .../skeleton_modification_3d_fabrik.cpp | 76 +++++++++---------- .../skeleton_modification_3d_fabrik.h | 2 + .../skeleton_modification_3d_jiggle.cpp | 7 +- .../skeleton_modification_3d_lookat.cpp | 4 +- .../skeleton_modification_3d_twoboneik.cpp | 26 ++++++- 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 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_data_chain; + LocalVector 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;