diff options
Diffstat (limited to 'scene/3d/skeleton_ik_3d.cpp')
| -rw-r--r-- | scene/3d/skeleton_ik_3d.cpp | 69 |
1 files changed, 16 insertions, 53 deletions
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(); |