Merge pull request #40774 from TwistedTwigleg/SkeletonIK_Godot_4_0_Fixes
SkeletonIK changes and bug fixes
This commit is contained in:
commit
a19ffe80da
|
@ -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.bone = p_task->root_bone;
|
||||||
chain.chain_root.initial_transform = p_task->skeleton->get_bone_global_pose(chain.chain_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.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;
|
chain.middle_chain_item = nullptr;
|
||||||
|
|
||||||
// Holds all IDs that are composing a single chain in reverse order
|
// 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) {
|
if (!child_ci) {
|
||||||
child_ci = sub_chain->add_child(chain_ids[i]);
|
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->initial_transform = p_task->skeleton->get_bone_global_pose(child_ci->bone);
|
||||||
child_ci->current_pos = child_ci->initial_transform.origin;
|
child_ci->current_pos = child_ci->initial_transform.origin;
|
||||||
|
|
||||||
|
@ -132,20 +129,6 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
|
||||||
return true;
|
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) {
|
||||||
real_t distance_to_goal(1e4);
|
real_t distance_to_goal(1e4);
|
||||||
real_t previous_distance_to_goal(0);
|
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;
|
p_task->end_effectors.write[0].goal_transform = p_inverse_transf * p_task->goal_global_transform;
|
||||||
} else {
|
} else {
|
||||||
// End effector in local transform
|
// 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
|
// 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);
|
p_task->end_effectors.write[0].goal_transform = end_effector_pose.interpolate_with(p_inverse_transf * p_task->goal_global_transform, blending_delta);
|
||||||
|
@ -285,9 +268,7 @@ 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);
|
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);
|
make_goal(p_task, p_task->skeleton->get_global_transform().affine_inverse(), blending_delta);
|
||||||
|
|
||||||
update_chain(p_task->skeleton, &p_task->chain.chain_root);
|
|
||||||
|
|
||||||
if (p_use_magnet && p_task->chain.middle_chain_item) {
|
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);
|
p_task->chain.magnet_position = p_task->chain.middle_chain_item->initial_transform.origin.lerp(p_magnet_position, blending_delta);
|
||||||
|
@ -310,6 +291,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)));
|
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);
|
new_bone_pose.basis.rotate(rot_axis, rot_angle);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// Set target orientation to tip
|
// Set target orientation to tip
|
||||||
if (override_tip_basis) {
|
if (override_tip_basis) {
|
||||||
|
@ -319,6 +301,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);
|
p_task->skeleton->set_bone_global_pose_override(ci->bone, new_bone_pose, 1.0, true);
|
||||||
|
|
||||||
if (!ci->children.is_empty()) {
|
if (!ci->children.is_empty()) {
|
||||||
|
|
|
@ -52,7 +52,6 @@ class FabrikInverseKinematic {
|
||||||
|
|
||||||
// Bone info
|
// Bone info
|
||||||
BoneId bone = -1;
|
BoneId bone = -1;
|
||||||
PhysicalBone3D *pb = nullptr;
|
|
||||||
|
|
||||||
real_t length = 0.0;
|
real_t length = 0.0;
|
||||||
/// Positions relative to root bone
|
/// Positions relative to root bone
|
||||||
|
@ -107,8 +106,6 @@ private:
|
||||||
/// Init a chain that starts from the root to tip
|
/// Init a chain that starts from the root to tip
|
||||||
static bool build_chain(Task *p_task, bool p_force_simple_chain = true);
|
static bool build_chain(Task *p_task, bool p_force_simple_chain = true);
|
||||||
|
|
||||||
static void update_chain(const Skeleton3D *p_sk, ChainItem *p_chain_item);
|
|
||||||
|
|
||||||
static void solve_simple(Task *p_task, bool p_solve_magnet);
|
static void solve_simple(Task *p_task, bool p_solve_magnet);
|
||||||
/// Special solvers that solve only chains with one end effector
|
/// Special solvers that solve only chains with one end effector
|
||||||
static void solve_simple_backwards(Chain &r_chain, bool p_solve_magnet);
|
static void solve_simple_backwards(Chain &r_chain, bool p_solve_magnet);
|
||||||
|
|
Loading…
Reference in a new issue