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.cpp104
1 files changed, 36 insertions, 68 deletions
diff --git a/scene/3d/skeleton_ik_3d.cpp b/scene/3d/skeleton_ik_3d.cpp
index 6cde6a9b17..1005d51e63 100644
--- a/scene/3d/skeleton_ik_3d.cpp
+++ b/scene/3d/skeleton_ik_3d.cpp
@@ -129,7 +129,7 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
return true;
}
-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);
@@ -138,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();
}
@@ -176,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;
@@ -212,7 +212,7 @@ void FabrikInverseKinematic::solve_simple_forwards(Chain &r_chain, bool p_solve_
}
}
-FabrikInverseKinematic::Task *FabrikInverseKinematic::create_simple_task(Skeleton3D *p_sk, BoneId root_bone, BoneId tip_bone, const Transform &goal_transform) {
+FabrikInverseKinematic::Task *FabrikInverseKinematic::create_simple_task(Skeleton3D *p_sk, BoneId root_bone, BoneId tip_bone, const Transform3D &goal_transform) {
FabrikInverseKinematic::EndEffector ee;
ee.tip_bone = tip_bone;
@@ -236,17 +236,17 @@ void FabrikInverseKinematic::free_task(Task *p_task) {
}
}
-void FabrikInverseKinematic::set_goal(Task *p_task, const Transform &p_goal) {
+void FabrikInverseKinematic::set_goal(Task *p_task, const Transform3D &p_goal) {
p_task->goal_global_transform = p_goal;
}
-void FabrikInverseKinematic::make_goal(Task *p_task, const Transform &p_inverse_transf, real_t blending_delta) {
+void FabrikInverseKinematic::make_goal(Task *p_task, const Transform3D &p_inverse_transf, real_t blending_delta) {
if (blending_delta >= 0.99f) {
// Update the end_effector (local transform) without blending
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 Transform3D 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,77 +270,42 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
return; // Skip solving
}
- // This line below is part of the problem - removing it fixes the issue with BoneAttachment nodes...
- 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);
+ p_task->skeleton->set_bone_global_pose_override(p_task->chain.chain_root.bone, Transform3D(), 0.0, false);
+ Vector3 origin_pos = p_task->skeleton->get_bone_global_pose(p_task->chain.chain_root.bone).origin;
+
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);
while (ci) {
- Transform new_bone_pose(ci->initial_transform);
+ Transform3D 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;
}
}
@@ -363,7 +328,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();
@@ -432,7 +397,7 @@ void SkeletonIK3D::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "root_bone"), "set_root_bone", "get_root_bone");
ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "tip_bone"), "set_tip_bone", "get_tip_bone");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "interpolation", PROPERTY_HINT_RANGE, "0,1,0.001"), "set_interpolation", "get_interpolation");
- ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM, "target"), "set_target_transform", "get_target_transform");
+ ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM3D, "target"), "set_target_transform", "get_target_transform");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "override_tip_basis"), "set_override_tip_basis", "is_override_tip_basis");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_magnet"), "set_use_magnet", "is_using_magnet");
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "magnet"), "set_magnet_position", "get_magnet_position");
@@ -496,12 +461,12 @@ real_t SkeletonIK3D::get_interpolation() const {
return interpolation;
}
-void SkeletonIK3D::set_target_transform(const Transform &p_target) {
+void SkeletonIK3D::set_target_transform(const Transform3D &p_target) {
target = p_target;
reload_goal();
}
-const Transform &SkeletonIK3D::get_target_transform() const {
+const Transform3D &SkeletonIK3D::get_target_transform() const {
return target;
}
@@ -567,9 +532,12 @@ 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() {
+Transform3D SkeletonIK3D::_get_target_transform() {
if (!target_node_override && !target_node_path_override.is_empty()) {
target_node_override = Object::cast_to<Node3D>(get_node(target_node_path_override));
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-15 15:03:13 +0000