Skip to content

Commit

Permalink
Merge pull request #48166 from TwistedTwigleg/skeletonik_changes_and_…
Browse files Browse the repository at this point in the history 
…bug_fixes_regressionfix3

Fixed issue in SkeletonIK leading to some root bones being twisted incorrectly
  • Loading branch information
@akien-mga
akien-mga authored May 8, 2021
2 parents 60ed7d0 + 446460e commit 7050e4d
Show file tree
Hide file tree
Showing 4 changed files with 77 additions and 90 deletions.
9 changes: 9 additions & 0 deletions doc/classes/Skeleton3D.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,15 @@
Returns the overall transform of the specified bone, with respect to the skeleton. Being relative to the skeleton frame, this is not the actual "global" transform of the bone.
</description>
</method>
<method name="get_bone_global_pose_no_override" qualifiers="const">
<return type="Transform">
</return>
<argument index="0" name="bone_idx" type="int">
</argument>
<description>
Returns the overall transform of the specified bone, with respect to the skeleton, but without any global pose overrides. Being relative to the skeleton frame, this is not the actual "global" transform of the bone.
</description>
</method>
<method name="get_bone_name" qualifiers="const">
<return type="String">
</return>
Expand Down
87 changes: 50 additions & 37 deletions scene/3d/skeleton_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,53 +237,57 @@ void Skeleton3D::_notification(int p_what) {
for (int i = 0; i < len; i++) {
Bone &b = bonesptr[order[i]];

if (b.global_pose_override_amount >= 0.999) {
b.pose_global = b.global_pose_override;
} else {
if (b.disable_rest) {
if (b.enabled) {
Transform pose = b.pose;
if (b.custom_pose_enable) {
pose = b.custom_pose * pose;
}
if (b.parent >= 0) {
b.pose_global = bonesptr[b.parent].pose_global * pose;
} else {
b.pose_global = pose;
}
if (b.disable_rest) {
if (b.enabled) {
Transform pose = b.pose;
if (b.custom_pose_enable) {
pose = b.custom_pose * pose;
}
if (b.parent >= 0) {
b.pose_global = bonesptr[b.parent].pose_global * pose;
b.pose_global_no_override = bonesptr[b.parent].pose_global * pose;
} else {
if (b.parent >= 0) {
b.pose_global = bonesptr[b.parent].pose_global;
} else {
b.pose_global = Transform();
}
b.pose_global = pose;
b.pose_global_no_override = pose;
}

} else {
if (b.enabled) {
Transform pose = b.pose;
if (b.custom_pose_enable) {
pose = b.custom_pose * pose;
}
if (b.parent >= 0) {
b.pose_global = bonesptr[b.parent].pose_global * (b.rest * pose);
} else {
b.pose_global = b.rest * pose;
}
if (b.parent >= 0) {
b.pose_global = bonesptr[b.parent].pose_global;
b.pose_global_no_override = bonesptr[b.parent].pose_global;
} else {
if (b.parent >= 0) {
b.pose_global = bonesptr[b.parent].pose_global * b.rest;
} else {
b.pose_global = b.rest;
}
b.pose_global = Transform();
b.pose_global_no_override = Transform();
}
}

if (b.global_pose_override_amount >= CMP_EPSILON) {
b.pose_global = b.pose_global.interpolate_with(b.global_pose_override, b.global_pose_override_amount);
} else {
if (b.enabled) {
Transform pose = b.pose;
if (b.custom_pose_enable) {
pose = b.custom_pose * pose;
}
if (b.parent >= 0) {
b.pose_global = bonesptr[b.parent].pose_global * (b.rest * pose);
b.pose_global_no_override = bonesptr[b.parent].pose_global * (b.rest * pose);
} else {
b.pose_global = b.rest * pose;
b.pose_global_no_override = b.rest * pose;
}
} else {
if (b.parent >= 0) {
b.pose_global = bonesptr[b.parent].pose_global * b.rest;
b.pose_global_no_override = bonesptr[b.parent].pose_global * b.rest;
} else {
b.pose_global = b.rest;
b.pose_global_no_override = b.rest;
}
}
}

if (b.global_pose_override_amount >= CMP_EPSILON) {
b.pose_global = b.pose_global.interpolate_with(b.global_pose_override, b.global_pose_override_amount);
}

if (b.global_pose_override_reset) {
b.global_pose_override_amount = 0.0;
}
Expand Down Expand Up @@ -408,6 +412,14 @@ Transform Skeleton3D::get_bone_global_pose(int p_bone) const {
return bones[p_bone].pose_global;
}

Transform Skeleton3D::get_bone_global_pose_no_override(int p_bone) const {
ERR_FAIL_INDEX_V(p_bone, bones.size(), Transform());
if (dirty) {
const_cast<Skeleton3D *>(this)->notification(NOTIFICATION_UPDATE_SKELETON);
}
return bones[p_bone].pose_global_no_override;
}

// skeleton creation api
void Skeleton3D::add_bone(const String &p_name) {
ERR_FAIL_COND(p_name == "" || p_name.find(":") != -1 || p_name.find("/") != -1);
Expand Down Expand Up @@ -912,6 +924,7 @@ void Skeleton3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("clear_bones_global_pose_override"), &Skeleton3D::clear_bones_global_pose_override);
ClassDB::bind_method(D_METHOD("set_bone_global_pose_override", "bone_idx", "pose", "amount", "persistent"), &Skeleton3D::set_bone_global_pose_override, DEFVAL(false));
ClassDB::bind_method(D_METHOD("get_bone_global_pose", "bone_idx"), &Skeleton3D::get_bone_global_pose);
ClassDB::bind_method(D_METHOD("get_bone_global_pose_no_override", "bone_idx"), &Skeleton3D::get_bone_global_pose_no_override);

ClassDB::bind_method(D_METHOD("get_bone_custom_pose", "bone_idx"), &Skeleton3D::get_bone_custom_pose);
ClassDB::bind_method(D_METHOD("set_bone_custom_pose", "bone_idx", "custom_pose"), &Skeleton3D::set_bone_custom_pose);
Expand Down
2 changes: 2 additions & 0 deletions scene/3d/skeleton_3d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,7 @@ class Skeleton3D : public Node3D {

Transform pose;
Transform pose_global;
Transform pose_global_no_override;

bool custom_pose_enable = false;
Transform custom_pose;
Expand Down Expand Up @@ -160,6 +161,7 @@ class Skeleton3D : public Node3D {
void set_bone_rest(int p_bone, const Transform &p_rest);
Transform get_bone_rest(int p_bone) const;
Transform get_bone_global_pose(int p_bone) const;
Transform get_bone_global_pose_no_override(int p_bone) const;

void clear_bones_global_pose_override();
void set_bone_global_pose_override(int p_bone, const Transform &p_pose, float p_amount, bool p_persistent = false);
Expand Down
69 changes: 16 additions & 53 deletions scene/3d/skeleton_ik_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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;
}
}

Expand All @@ -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();
Expand Down

0 comments on commit 7050e4d

Please sign in to comment.