summaryrefslogtreecommitdiff
path: root/scene/3d/skeleton_ik_3d.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'scene/3d/skeleton_ik_3d.cpp')
-rw-r--r--scene/3d/skeleton_ik_3d.cpp111
1 files changed, 78 insertions, 33 deletions
diff --git a/scene/3d/skeleton_ik_3d.cpp b/scene/3d/skeleton_ik_3d.cpp
index cb486a12ae..898f94ccc1 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,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);
@@ -272,6 +255,18 @@ 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) {
+ // 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);
+
+ if (!ci->children.is_empty()) {
+ ci = &ci->children.write[0];
+ } else {
+ ci = nullptr;
+ }
+ }
+
return; // Skip solving
}
@@ -285,9 +280,11 @@ 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 the transforms to their global poses
+ // (Needed to sync IK with animation)
+ _update_chain(p_task->skeleton, &p_task->chain.chain_root);
- 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);
@@ -301,24 +298,55 @@ 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;
- 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);
+ // 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 {
- // Set target orientation to tip
- if (override_tip_basis) {
- new_bone_pose.basis = p_task->chain.tips[0].end_effector->goal_transform.basis;
+ 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);
+ }
+
} else {
- new_bone_pose.basis = new_bone_pose.basis * p_task->chain.tips[0].end_effector->goal_transform.basis;
+ // 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;
+ }
}
}
+ // 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 +357,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(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 +566,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() {
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-11 07:03:52 +0000