diff options
Diffstat (limited to 'scene/3d/skeleton_ik_3d.cpp')
| -rw-r--r-- | scene/3d/skeleton_ik_3d.cpp | 81 |
1 files changed, 46 insertions, 35 deletions
diff --git a/scene/3d/skeleton_ik_3d.cpp b/scene/3d/skeleton_ik_3d.cpp index cb486a12ae..294e313300 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; @@ -123,7 +120,7 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain if (p_force_simple_chain) { // NOTE: - // This is an "hack" that force to create only one tip per chain since the solver of multi tip (end effector) + // This is a "hack" that force to create only one tip per chain since the solver of multi tip (end effector) // is not yet created. // Remove this code when this is done break; @@ -132,21 +129,7 @@ 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) { +void FabrikInverseKinematic::solve_simple(Task *p_task, bool p_solve_magnet, Vector3 p_origin_pos) { real_t distance_to_goal(1e4); real_t previous_distance_to_goal(0); int can_solve(p_task->max_iterations); @@ -155,7 +138,7 @@ void FabrikInverseKinematic::solve_simple(Task *p_task, bool p_solve_magnet) { --can_solve; solve_simple_backwards(p_task->chain, p_solve_magnet); - solve_simple_forwards(p_task->chain, p_solve_magnet); + solve_simple_forwards(p_task->chain, p_solve_magnet, p_origin_pos); distance_to_goal = (p_task->chain.tips[0].chain_item->current_pos - p_task->chain.tips[0].end_effector->goal_transform.origin).length(); } @@ -193,13 +176,13 @@ void FabrikInverseKinematic::solve_simple_backwards(Chain &r_chain, bool p_solve } } -void FabrikInverseKinematic::solve_simple_forwards(Chain &r_chain, bool p_solve_magnet) { +void FabrikInverseKinematic::solve_simple_forwards(Chain &r_chain, bool p_solve_magnet, Vector3 p_origin_pos) { if (p_solve_magnet && !r_chain.middle_chain_item) { return; } ChainItem *sub_chain_root(&r_chain.chain_root); - Vector3 origin(r_chain.chain_root.initial_transform.origin); + Vector3 origin = p_origin_pos; while (sub_chain_root) { // Reach the tip sub_chain_root->current_pos = origin; @@ -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_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); @@ -272,28 +255,34 @@ void FabrikInverseKinematic::make_goal(Task *p_task, const Transform &p_inverse_ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool override_tip_basis, bool p_use_magnet, const Vector3 &p_magnet_position) { if (blending_delta <= 0.01f) { - return; // Skip solving - } + // Before skipping, make sure we undo the global pose overrides + ChainItem *ci(&p_task->chain.chain_root); + while (ci) { + p_task->skeleton->set_bone_global_pose_override(ci->bone, ci->initial_transform, 0.0, false); - p_task->skeleton->set_bone_global_pose_override(p_task->chain.chain_root.bone, Transform(), 0.0, true); + if (!ci->children.is_empty()) { + ci = &ci->children.write[0]; + } else { + ci = nullptr; + } + } - 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); + return; // Skip solving } - 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 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().scaled(p_task->skeleton->get_global_transform().get_basis().get_scale()), blending_delta); + p_task->skeleton->set_bone_global_pose_override(p_task->chain.chain_root.bone, Transform(), 0.0, false); + Vector3 origin_pos = p_task->skeleton->get_bone_global_pose(p_task->chain.chain_root.bone).origin; - 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); - solve_simple(p_task, true); + solve_simple(p_task, true, origin_pos); } - solve_simple(p_task, false); + solve_simple(p_task, false, origin_pos); // Assign new bone position. ChainItem *ci(&p_task->chain.chain_root); @@ -310,6 +299,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 +309,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.is_empty()) { @@ -329,6 +323,20 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove } } +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_no_override(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 SkeletonIK3D::_validate_property(PropertyInfo &property) const { if (property.name == "root_bone" || property.name == "tip_bone") { if (skeleton) { @@ -524,6 +532,9 @@ void SkeletonIK3D::start(bool p_one_time) { void SkeletonIK3D::stop() { set_process_internal(false); + if (skeleton) { + skeleton->clear_bones_global_pose_override(); + } } Transform SkeletonIK3D::_get_target_transform() { |