diff --git a/doc/classes/IKModifier3D.xml b/doc/classes/IKModifier3D.xml index 0ad6bccb9cb3..5d22fddf60bd 100644 --- a/doc/classes/IKModifier3D.xml +++ b/doc/classes/IKModifier3D.xml @@ -5,7 +5,6 @@ Base class of [SkeletonModifier3D]s that has some joint lists and applies inverse kinematics. This class has some structs, enums, and helper methods which are useful to solve inverse kinematics. - [b]Note:[/b] The IK classes that extend this handle rotation only, with bone lengths cached. It means that a position movement between processed chains can cause unintended movement. @@ -36,4 +35,10 @@ + + + If [code]true[/code], the solver retrieves the bone axis from the bone pose every frame. + If [code]false[/code], the solver retrieves the bone axis from the bone rest and caches it, which increases performance slightly, but position changes in the bone pose made before processing this [IKModifier3D] are ignored. + + diff --git a/doc/classes/IterateIK3D.xml b/doc/classes/IterateIK3D.xml index 76f7148f41a5..1b380704cba9 100644 --- a/doc/classes/IterateIK3D.xml +++ b/doc/classes/IterateIK3D.xml @@ -145,6 +145,10 @@ The maximum amount each bone can rotate in a single iteration. [b]Note:[/b] This limitation is applied during each iteration. For example, if [member max_iterations] is [code]4[/code] and [member angular_delta_limit] is [code]5[/code] degrees, the maximum rotation possible in a single frame is [code]20[/code] degrees. + + If [code]false[/code], the result is calculated from the previous frame's [IterateIK3D] result as the initial state. + If [code]true[/code], the previous frame's [IterateIK3D] result is discarded. At this point, the new result is calculated from the bone pose excluding the [IterateIK3D] as the initial state. This means the result will be always equal as long as the target position and the previous bone pose are the same. However, if [member angular_delta_limit] and [member max_iterations] are set too small, the end bone of the chain will never reach the target. + The number of iteration loops used by the IK solver to produce more accurate results. diff --git a/doc/classes/SpringBoneSimulator3D.xml b/doc/classes/SpringBoneSimulator3D.xml index 914ebfeddd45..f1cbc63b19a0 100644 --- a/doc/classes/SpringBoneSimulator3D.xml +++ b/doc/classes/SpringBoneSimulator3D.xml @@ -637,6 +637,10 @@ The constant force that always affected bones. It is equal to the result when the parent [Skeleton3D] moves at this speed in the opposite direction. This is useful for effects such as wind and anti-gravity. + + If [code]true[/code], the solver retrieves the bone axis from the bone pose every frame. + If [code]false[/code], the solver retrieves the bone axis from the bone rest and caches it, which increases performance slightly, but position changes in the bone pose made before processing this [SpringBoneSimulator3D] are ignored. + The number of settings. diff --git a/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp b/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp index 3b119e1eb849..98a2801a87ee 100644 --- a/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp +++ b/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp @@ -135,13 +135,18 @@ Ref ChainIK3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, Cha int current_bone = -1; int prev_bone = -1; int joint_end = p_ik->get_joint_count(i) - 1; + float prev_length = INFINITY; bool is_extended = p_ik->is_end_bone_extended(i) && p_ik->get_end_bone_length(i) > 0; + Transform3D anc_global_pose = p_ik->get_chain_root_global_rest(i); for (int j = 0; j <= joint_end; j++) { current_bone = p_ik->get_joint_bone(i, j); - Transform3D global_pose = p_skeleton->get_bone_global_rest(current_bone); if (j > 0) { + int prev_joint = j - 1; Transform3D parent_global_pose = p_skeleton->get_bone_global_rest(prev_bone); - draw_line(surface_tool, parent_global_pose.origin, global_pose.origin, bone_color); + Vector3 bone_vector = p_ik->get_bone_vector(i, prev_joint); + float current_length = bone_vector.length(); + Vector3 center = parent_global_pose.translated_local(bone_vector).origin; + draw_line(surface_tool, parent_global_pose.origin, center, bone_color); if (it_ik) { // Draw rotation axis vector if not ROTATION_AXIS_ALL. @@ -150,15 +155,14 @@ Ref ChainIK3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, Cha if (rotation_axis != SkeletonModifier3D::ROTATION_AXIS_ALL) { Vector3 axis_vector = it_ik->get_joint_rotation_axis_vector(i, j); if (!axis_vector.is_zero_approx()) { - float rot_axis_length = (global_pose.origin - parent_global_pose.origin).length() * 0.2; // Use 20% of the bone length for the rotation axis vector. - Vector3 axis = global_pose.basis.xform(axis_vector.normalized()) * rot_axis_length; - draw_line(surface_tool, global_pose.origin - axis, global_pose.origin + axis, bone_color); + float rot_axis_length = bone_vector.length() * 0.2; // Use 20% of the bone length for the rotation axis vector. + Vector3 axis = parent_global_pose.basis.xform(axis_vector.normalized()) * rot_axis_length; + draw_line(surface_tool, center - axis, center + axis, bone_color); } } } // Draw parent limitation shape. - int prev_joint = j - 1; Ref lim = it_ik->get_joint_limitation(i, prev_joint); if (lim.is_valid()) { // Limitation space should bind parent bone rest. @@ -170,28 +174,36 @@ Ref ChainIK3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, Cha surface_tool->set_weights(weights); } } - Transform3D tr = parent_global_pose; - Vector3 forward = p_skeleton->get_bone_rest(current_bone).origin; - tr.basis *= it_ik->get_joint_limitation_space(i, prev_joint, forward); - lim->draw_shape(surface_tool, tr, forward.length(), bone_color); - Vector3 x_axis = tr.basis.get_column(Vector3::AXIS_X).normalized() * forward.length() * 0.1; - Vector3 z_axis = tr.basis.get_column(Vector3::AXIS_Z).normalized() * forward.length() * 0.1; + Transform3D tr = anc_global_pose; + tr.basis *= it_ik->get_joint_limitation_space(i, prev_joint, bone_vector.normalized()); + float sl = MIN(current_length, prev_length); + lim->draw_shape(surface_tool, tr, sl, bone_color); + sl *= 0.1; + Vector3 x_axis = tr.basis.get_column(Vector3::AXIS_X).normalized() * sl; + Vector3 z_axis = tr.basis.get_column(Vector3::AXIS_Z).normalized() * sl; draw_line(surface_tool, tr.origin + x_axis * 2, tr.origin + x_axis * 3, limitation_x_axis_color); // Offset 20%. draw_line(surface_tool, tr.origin + z_axis * 2, tr.origin + z_axis * 3, limitation_z_axis_color); // Offset 20%. } } + prev_length = current_length; + Transform3D tr = p_skeleton->get_bone_rest(current_bone); + tr.origin = bone_vector; + parent_global_pose *= tr; + anc_global_pose = parent_global_pose; } if (j == joint_end && is_extended) { - Vector3 axis = p_ik->get_bone_axis(current_bone, p_ik->get_end_bone_direction(i)); - if (axis.is_zero_approx()) { + Transform3D current_global_pose = p_skeleton->get_bone_global_rest(current_bone); + Vector3 bone_vector = p_ik->get_bone_vector(i, j); + if (bone_vector.is_zero_approx()) { continue; } + float current_length = bone_vector.length(); bones.write[0] = current_bone; surface_tool->set_bones(bones); surface_tool->set_weights(weights); - float length = p_ik->get_end_bone_length(i); - axis = global_pose.xform(axis * length); - draw_line(surface_tool, global_pose.origin, axis, bone_color); + Vector3 center = current_global_pose.translated_local(bone_vector).origin; + draw_line(surface_tool, current_global_pose.origin, center, bone_color); + if (it_ik) { // Draw limitation shape. Ref lim = it_ik->get_joint_limitation(i, j); @@ -205,12 +217,13 @@ Ref ChainIK3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, Cha surface_tool->set_weights(weights); } } - Vector3 forward = it_ik->get_bone_axis(current_bone, it_ik->get_end_bone_direction(i)); - Transform3D tr = global_pose; - tr.basis *= it_ik->get_joint_limitation_space(i, j, forward); - lim->draw_shape(surface_tool, tr, length, bone_color); - Vector3 x_axis = tr.basis.get_column(Vector3::AXIS_X).normalized() * length * 0.1; - Vector3 z_axis = tr.basis.get_column(Vector3::AXIS_Z).normalized() * length * 0.1; + Transform3D tr = anc_global_pose; + tr.basis *= it_ik->get_joint_limitation_space(i, j, bone_vector.normalized()); + float sl = MIN(current_length, prev_length); + lim->draw_shape(surface_tool, tr, sl, bone_color); + sl *= 0.1; + Vector3 x_axis = tr.basis.get_column(Vector3::AXIS_X).normalized() * sl; + Vector3 z_axis = tr.basis.get_column(Vector3::AXIS_Z).normalized() * sl; draw_line(surface_tool, tr.origin + x_axis * 2, tr.origin + x_axis * 3, limitation_x_axis_color); // Offset 20%. draw_line(surface_tool, tr.origin + z_axis * 2, tr.origin + z_axis * 3, limitation_z_axis_color); // Offset 20%. } @@ -231,10 +244,10 @@ Ref ChainIK3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, Cha if (rotation_axis != SkeletonModifier3D::ROTATION_AXIS_ALL) { Vector3 axis_vector = it_ik->get_joint_rotation_axis_vector(i, j); if (!axis_vector.is_zero_approx()) { - Transform3D next_bone_global_pose = p_skeleton->get_bone_global_rest(it_ik->get_joint_bone(i, 1)); - float rot_axis_length = (next_bone_global_pose.origin - global_pose.origin).length() * 0.2; // Use 20% of the bone length for the rotation axis vector. - Vector3 axis = global_pose.basis.xform(axis_vector.normalized()) * rot_axis_length; - draw_line(surface_tool, global_pose.origin - axis, global_pose.origin + axis, bone_color); + Vector3 bone_vector = p_ik->get_bone_vector(i, j); + float rot_axis_length = bone_vector.length() * 0.2; // Use 20% of the bone length for the rotation axis vector. + Vector3 axis = anc_global_pose.basis.xform(axis_vector.normalized()) * rot_axis_length; + draw_line(surface_tool, anc_global_pose.origin - axis, anc_global_pose.origin + axis, bone_color); } } } diff --git a/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp b/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp index f914aaff11e5..f4d06c80a2f4 100644 --- a/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp +++ b/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp @@ -142,8 +142,10 @@ Ref SpringBoneSimulator3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_s Transform3D global_pose = p_skeleton->get_bone_global_rest(current_bone); if (j > 0) { Transform3D parent_global_pose = p_skeleton->get_bone_global_rest(prev_bone); - draw_line(surface_tool, parent_global_pose.origin, global_pose.origin, bone_color); - draw_sphere(surface_tool, global_pose.basis, global_pose.origin, p_simulator->get_joint_radius(i, j - 1), bone_color); + Vector3 bone_vector = p_simulator->get_bone_vector(i, j - 1); + Vector3 center = parent_global_pose.translated_local(bone_vector).origin; + draw_line(surface_tool, parent_global_pose.origin, center, bone_color); + draw_sphere(surface_tool, global_pose.basis, center, p_simulator->get_joint_radius(i, j - 1), bone_color); // Draw rotation axis vector if not ROTATION_AXIS_ALL. if (j != joint_end || (j == joint_end && is_extended)) { @@ -153,22 +155,22 @@ Ref SpringBoneSimulator3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_s if (!axis_vector.is_zero_approx()) { float line_length = p_simulator->get_joint_radius(i, j - 1) * 2.0; Vector3 axis = global_pose.basis.xform(axis_vector.normalized()) * line_length; - draw_line(surface_tool, global_pose.origin - axis, global_pose.origin + axis, bone_color); + draw_line(surface_tool, center - axis, center + axis, bone_color); } } } } if (j == joint_end && is_extended) { - Vector3 axis = p_simulator->get_end_bone_axis(current_bone, p_simulator->get_end_bone_direction(i)); - if (axis.is_zero_approx()) { + Vector3 bone_vector = p_simulator->get_bone_vector(i, j); + if (bone_vector.is_zero_approx()) { continue; } bones[0] = current_bone; surface_tool->set_bones(Vector(bones)); surface_tool->set_weights(Vector(weights)); - axis = global_pose.xform(axis * p_simulator->get_end_bone_length(i)); - draw_line(surface_tool, global_pose.origin, axis, bone_color); - draw_sphere(surface_tool, global_pose.basis, axis, p_simulator->get_joint_radius(i, j), bone_color); + Vector3 center = global_pose.translated_local(bone_vector).origin; + draw_line(surface_tool, global_pose.origin, center, bone_color); + draw_sphere(surface_tool, global_pose.basis, center, p_simulator->get_joint_radius(i, j), bone_color); } else { bones[0] = current_bone; surface_tool->set_bones(Vector(bones)); diff --git a/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp b/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp index 101b96f49090..9ae8e053b558 100644 --- a/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp +++ b/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp @@ -134,35 +134,28 @@ Ref TwoBoneIK3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, T int root_bone = p_ik->get_root_bone(i); int middle_bone = p_ik->get_middle_bone(i); - int end_bone = p_ik->get_end_bone(i); - - bool is_extended = p_ik->is_end_bone_extended(i) && p_ik->get_end_bone_length(i) > 0.0f; Transform3D root_gp = p_skeleton->get_bone_global_rest(root_bone); Transform3D mid_gp = p_skeleton->get_bone_global_rest(middle_bone); - Transform3D end_gp = p_skeleton->get_bone_global_rest(end_bone); - - Vector3 end_point = end_gp.origin; - if (is_extended) { - end_point += end_gp.basis.get_rotation_quaternion().xform(p_ik->get_bone_axis(end_bone, p_ik->get_end_bone_direction(i))).normalized() * p_ik->get_end_bone_length(i); - } + Vector3 root_vec = p_ik->get_root_bone_vector(i); + Vector3 mid_vec = p_ik->get_middle_bone_vector(i); bones.write[0] = root_bone; surface_tool->set_bones(bones); surface_tool->set_weights(weights); - draw_line(surface_tool, root_gp.origin, mid_gp.origin, bone_color); + draw_line(surface_tool, root_gp.origin, root_gp.translated_local(root_vec).origin, bone_color); bones.write[0] = middle_bone; surface_tool->set_bones(bones); surface_tool->set_weights(weights); - draw_line(surface_tool, mid_gp.origin, end_point, bone_color); + draw_line(surface_tool, mid_gp.origin, mid_gp.translated_local(mid_vec).origin, bone_color); Vector3 pole_vector = p_ik->get_pole_direction_vector(i); if (pole_vector.is_zero_approx()) { continue; } - float pole_length = MIN(root_gp.origin.distance_to(mid_gp.origin), mid_gp.origin.distance_to(end_point)) * 0.25; + float pole_length = MIN(root_vec.length(), mid_vec.length()) * 0.25; draw_arrow(surface_tool, mid_gp.origin, mid_gp.basis.get_rotation_quaternion().xform(pole_vector).normalized(), pole_length, bone_color); } diff --git a/scene/3d/bone_constraint_3d.cpp b/scene/3d/bone_constraint_3d.cpp index 5bad937633ed..cc04a5595a72 100644 --- a/scene/3d/bone_constraint_3d.cpp +++ b/scene/3d/bone_constraint_3d.cpp @@ -160,7 +160,7 @@ void BoneConstraint3D::set_apply_bone(int p_index, int p_bone) { Skeleton3D *sk = get_skeleton(); if (sk) { if (settings[p_index]->apply_bone <= -1 || settings[p_index]->apply_bone >= sk->get_bone_count()) { - WARN_PRINT("apply bone index out of range!"); + WARN_PRINT("Apply bone index out of range!"); settings[p_index]->apply_bone = -1; } else { settings[p_index]->apply_bone_name = sk->get_bone_name(settings[p_index]->apply_bone); diff --git a/scene/3d/chain_ik_3d.cpp b/scene/3d/chain_ik_3d.cpp index eb132bec99d1..4f5d536617fb 100644 --- a/scene/3d/chain_ik_3d.cpp +++ b/scene/3d/chain_ik_3d.cpp @@ -258,7 +258,7 @@ void ChainIK3D::set_extend_end_bone(int p_index, bool p_enabled) { } notify_property_list_changed(); #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -270,13 +270,16 @@ bool ChainIK3D::is_end_bone_extended(int p_index) const { void ChainIK3D::set_end_bone_direction(int p_index, BoneDirection p_bone_direction) { ERR_FAIL_INDEX(p_index, (int)settings.size()); chain_settings[p_index]->end_bone_direction = p_bone_direction; - _make_simulation_dirty(p_index); Skeleton3D *sk = get_skeleton(); if (sk && !chain_settings[p_index]->joints.is_empty()) { _validate_axis(sk, p_index, chain_settings[p_index]->joints.size() - 1); } + if (mutable_bone_axes) { + return; // Chain dir will be recaluclated in _update_bone_axis(). + } + _make_simulation_dirty(p_index); #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -287,10 +290,15 @@ SkeletonModifier3D::BoneDirection ChainIK3D::get_end_bone_direction(int p_index) void ChainIK3D::set_end_bone_length(int p_index, float p_length) { ERR_FAIL_INDEX(p_index, (int)settings.size()); + float old = chain_settings[p_index]->end_bone_length; chain_settings[p_index]->end_bone_length = p_length; _make_simulation_dirty(p_index); + if (mutable_bone_axes && Math::is_zero_approx(old) == Math::is_zero_approx(p_length)) { + return; // If chain size is not changed, length will be recaluclated in _update_bone_axis(). + } + _make_simulation_dirty(p_index); #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -472,7 +480,7 @@ void ChainIK3D::_update_joints(int p_index) { } #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -480,6 +488,59 @@ void ChainIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) { // } +#ifdef TOOLS_ENABLED +void ChainIK3D::_update_mutable_info() { + if (!is_inside_tree()) { + return; + } + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + for (uint32_t i = 0; i < settings.size(); i++) { + chain_settings[i]->root_global_rest = Transform3D(); + } + } + bool changed = false; + for (uint32_t i = 0; i < settings.size(); i++) { + int root_bone = chain_settings[i]->root_bone.bone; + if (root_bone < 0) { + continue; + } + Transform3D new_tr = get_bone_global_rest_mutable(skeleton, root_bone); + changed = changed || !chain_settings[i]->root_global_rest.is_equal_approx(new_tr); + chain_settings[i]->root_global_rest = new_tr; + } + if (changed) { + _make_gizmo_dirty(); + } +} + +Transform3D ChainIK3D::get_bone_global_rest_mutable(Skeleton3D *p_skeleton, int p_bone) { + int current = p_bone; + Transform3D accum; + int parent = p_skeleton->get_bone_parent(current); + if (parent >= 0) { + accum = p_skeleton->get_bone_global_rest(parent); + } + Transform3D tr = p_skeleton->get_bone_rest(current); + // Note: + // Chain IK gizmo might not be able to retrieve this pose in SkeletonModifier update process. + // So the gizmo uses bone_vector insteads but parent of root bone doesn't have bone_vector. + // Then, we needs to cache this pose in IK node. + tr.origin = p_skeleton->get_bone_pose_position(current); + accum *= tr; + return accum; +} + +Transform3D ChainIK3D::get_chain_root_global_rest(int p_index) { + ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Transform3D()); + return chain_settings[p_index]->root_global_rest; +} + +Vector3 ChainIK3D::get_bone_vector(int p_index, int p_joint) const { + return Vector3(); +} +#endif // TOOLS_ENABLED + ChainIK3D::~ChainIK3D() { clear_settings(); } diff --git a/scene/3d/chain_ik_3d.h b/scene/3d/chain_ik_3d.h index 0209c324c634..e7bec7dfbee9 100644 --- a/scene/3d/chain_ik_3d.h +++ b/scene/3d/chain_ik_3d.h @@ -37,6 +37,15 @@ class ChainIK3D : public IKModifier3D { public: struct ChainIK3DSetting : public IKModifier3DSetting { +#ifdef TOOLS_ENABLED + // Note: + // To cache global rest on global pose in SkeletonModifier process. + // Since gizmo drawing might be processed after SkeletonModifier process, + // so the gizmo which depend on modified pose is not drawn correctly. + // Especially, limitation sphere is needed this since it bound mutable bone axis which retrieve by bone pose to the parent bone rest. + Transform3D root_global_rest; +#endif // TOOLS_ENABLED + BoneJoint root_bone; BoneJoint end_bone; @@ -121,12 +130,18 @@ class ChainIK3D : public IKModifier3D { int cur_head = p_index - 1; int cur_tail = p_index; if (cur_head >= 0) { - solver_info_list[cur_head]->current_vector = (chain[cur_tail] - chain[cur_head]).normalized(); + IKModifier3DSolverInfo *solver_info = solver_info_list[cur_head]; + if (solver_info) { + solver_info->current_vector = (chain[cur_tail] - chain[cur_head]).normalized(); + } } cur_head = p_index; cur_tail = p_index + 1; if (cur_tail < (int)chain.size()) { - solver_info_list[cur_head]->current_vector = (chain[cur_tail] - chain[cur_head]).normalized(); + IKModifier3DSolverInfo *solver_info = solver_info_list[cur_head]; + if (solver_info) { + solver_info->current_vector = (chain[cur_tail] - chain[cur_head]).normalized(); + } } } @@ -182,6 +197,10 @@ class ChainIK3D : public IKModifier3D { }; protected: +#ifdef TOOLS_ENABLED + virtual void _update_mutable_info() override; +#endif // TOOLS_ENABLED + LocalVector chain_settings; // For caching. bool _get(const StringName &p_path, Variant &r_ret) const; @@ -239,5 +258,12 @@ class ChainIK3D : public IKModifier3D { void set_joint_count(int p_index, int p_count); int get_joint_count(int p_index) const; +#ifdef TOOLS_ENABLED + // Helper. + static Transform3D get_bone_global_rest_mutable(Skeleton3D *p_skeleton, int p_bone); + Transform3D get_chain_root_global_rest(int p_index); + virtual Vector3 get_bone_vector(int p_index, int p_joint) const; +#endif // TOOLS_ENABLED + ~ChainIK3D(); }; diff --git a/scene/3d/ik_modifier_3d.cpp b/scene/3d/ik_modifier_3d.cpp index c0ca377b5dc6..2ec5c00e73fe 100644 --- a/scene/3d/ik_modifier_3d.cpp +++ b/scene/3d/ik_modifier_3d.cpp @@ -37,12 +37,14 @@ void IKModifier3D::_notification(int p_what) { if (Engine::get_singleton()->is_editor_hint()) { set_notify_local_transform(true); // Used for updating gizmo in editor. } + _update_mutable_info(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED _make_all_joints_dirty(); } break; #ifdef TOOLS_ENABLED case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: { - update_gizmos(); + _make_gizmo_dirty(); } break; #endif // TOOLS_ENABLED } @@ -53,8 +55,13 @@ void IKModifier3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_setting_count"), &IKModifier3D::get_setting_count); ClassDB::bind_method(D_METHOD("clear_settings"), &IKModifier3D::clear_settings); + ClassDB::bind_method(D_METHOD("set_mutable_bone_axes", "enabled"), &IKModifier3D::set_mutable_bone_axes); + ClassDB::bind_method(D_METHOD("are_bone_axes_mutable"), &IKModifier3D::are_bone_axes_mutable); + // To process manually. ClassDB::bind_method(D_METHOD("reset"), &IKModifier3D::reset); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "mutable_bone_axes"), "set_mutable_bone_axes", "are_bone_axes_mutable"); } void IKModifier3D::_set_active(bool p_active) { @@ -106,6 +113,44 @@ void IKModifier3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) { // } +void IKModifier3D::_update_bone_axis(Skeleton3D *p_skeleton, int p_index) { + // +} + +#ifdef TOOLS_ENABLED +void IKModifier3D::_make_gizmo_dirty() { + if (gizmo_dirty) { + return; + } + gizmo_dirty = true; + + callable_mp(this, &IKModifier3D::_redraw_gizmo).call_deferred(); +} + +void IKModifier3D::_update_mutable_info() { + // +} + +void IKModifier3D::_redraw_gizmo() { + update_gizmos(); + gizmo_dirty = false; +} +#endif // TOOLS_ENABLED + +void IKModifier3D::set_mutable_bone_axes(bool p_enabled) { + mutable_bone_axes = p_enabled; + for (uint32_t i = 0; i < settings.size(); i++) { + _make_simulation_dirty(i); + } +#ifdef TOOLS_ENABLED + _update_mutable_info(); +#endif // TOOLS_ENABLED +} + +bool IKModifier3D::are_bone_axes_mutable() const { + return mutable_bone_axes; +} + Quaternion IKModifier3D::get_local_pose_rotation(Skeleton3D *p_skeleton, int p_bone, const Quaternion &p_global_pose_rotation) { int parent = p_skeleton->get_bone_parent(p_bone); if (parent < 0) { @@ -114,15 +159,14 @@ Quaternion IKModifier3D::get_local_pose_rotation(Skeleton3D *p_skeleton, int p_b return p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion().inverse() * p_global_pose_rotation; } -Vector3 IKModifier3D::get_bone_axis(int p_end_bone, BoneDirection p_direction) const { - if (!is_inside_tree()) { +Vector3 IKModifier3D::get_bone_axis(Skeleton3D *p_skeleton, int p_end_bone, BoneDirection p_direction, bool p_mutable_bone_axes) { + if (!p_skeleton->is_inside_tree()) { return Vector3(); } Vector3 axis; if (p_direction == BONE_DIRECTION_FROM_PARENT) { - Skeleton3D *sk = get_skeleton(); - if (sk) { - axis = sk->get_bone_rest(p_end_bone).basis.xform_inv(sk->get_bone_rest(p_end_bone).origin); + if (p_skeleton) { + axis = p_skeleton->get_bone_rest(p_end_bone).basis.xform_inv(p_mutable_bone_axes ? p_skeleton->get_bone_pose(p_end_bone).origin : p_skeleton->get_bone_rest(p_end_bone).origin); axis.normalize(); } } else { diff --git a/scene/3d/ik_modifier_3d.h b/scene/3d/ik_modifier_3d.h index 2b9fa552dbf6..1e2e670d9dfd 100644 --- a/scene/3d/ik_modifier_3d.h +++ b/scene/3d/ik_modifier_3d.h @@ -40,6 +40,7 @@ class IKModifier3D : public SkeletonModifier3D { bool saving = false; #endif // TOOLS_ENABLED + bool mutable_bone_axes = true; Transform3D cached_space; bool joints_dirty = false; @@ -79,6 +80,14 @@ class IKModifier3D : public SkeletonModifier3D { virtual void _init_joints(Skeleton3D *p_skeleton, int p_index); virtual void _update_joints(int p_index); virtual void _make_simulation_dirty(int p_index); + virtual void _update_bone_axis(Skeleton3D *p_skeleton, int p_index); + +#ifdef TOOLS_ENABLED + bool gizmo_dirty = false; + void _make_gizmo_dirty(); + virtual void _update_mutable_info(); + void _redraw_gizmo(); +#endif // TOOLS_ENABLED virtual void _process_modification(double p_delta) override; virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta); @@ -121,9 +130,12 @@ class IKModifier3D : public SkeletonModifier3D { _set_setting_count(0); } + void set_mutable_bone_axes(bool p_enabled); + bool are_bone_axes_mutable() const; + // Helper. static Quaternion get_local_pose_rotation(Skeleton3D *p_skeleton, int p_bone, const Quaternion &p_global_pose_rotation); - Vector3 get_bone_axis(int p_end_bone, BoneDirection p_direction) const; + static Vector3 get_bone_axis(Skeleton3D *p_skeleton, int p_end_bone, BoneDirection p_direction, bool p_mutable_bone_axes); // To process manually. void reset(); diff --git a/scene/3d/iterate_ik_3d.cpp b/scene/3d/iterate_ik_3d.cpp index e49b71654055..8a86b867cedd 100644 --- a/scene/3d/iterate_ik_3d.cpp +++ b/scene/3d/iterate_ik_3d.cpp @@ -190,6 +190,14 @@ double IterateIK3D::get_angular_delta_limit() const { return angular_delta_limit; } +void IterateIK3D::set_deterministic(bool p_deterministic) { + deterministic = p_deterministic; +} + +bool IterateIK3D::is_deterministic() const { + return deterministic; +} + // Setting. void IterateIK3D::set_target_node(int p_index, const NodePath &p_node_path) { @@ -207,7 +215,7 @@ NodePath IterateIK3D::get_target_node(int p_index) const { void IterateIK3D::set_joint_rotation_axis(int p_index, int p_joint, RotationAxis p_axis) { ERR_FAIL_INDEX(p_index, (int)settings.size()); - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); joint_settings[p_joint]->rotation_axis = p_axis; Skeleton3D *sk = get_skeleton(); @@ -215,10 +223,7 @@ void IterateIK3D::set_joint_rotation_axis(int p_index, int p_joint, RotationAxis _validate_axis(sk, p_index, p_joint); } notify_property_list_changed(); - iterate_settings[p_index]->simulation_dirty = true; // Snapping to planes is needed in the initialization, so need to restructure. -#ifdef TOOLS_ENABLED - update_gizmos(); -#endif // TOOLS_ENABLED + _make_simulation_dirty(p_index); // Snapping to planes is needed in the initialization, so need to restructure. } SkeletonModifier3D::RotationAxis IterateIK3D::get_joint_rotation_axis(int p_index, int p_joint) const { @@ -230,17 +235,14 @@ SkeletonModifier3D::RotationAxis IterateIK3D::get_joint_rotation_axis(int p_inde void IterateIK3D::set_joint_rotation_axis_vector(int p_index, int p_joint, const Vector3 &p_vector) { ERR_FAIL_INDEX(p_index, (int)settings.size()); - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); joint_settings[p_joint]->rotation_axis_vector = p_vector; Skeleton3D *sk = get_skeleton(); if (sk) { _validate_axis(sk, p_index, p_joint); } - iterate_settings[p_index]->simulation_dirty = true; // Snapping to planes is needed in the initialization, so need to restructure. -#ifdef TOOLS_ENABLED - update_gizmos(); -#endif // TOOLS_ENABLED + _make_simulation_dirty(p_index); // Snapping to planes is needed in the initialization, so need to restructure. } Vector3 IterateIK3D::get_joint_rotation_axis_vector(int p_index, int p_joint) const { @@ -259,7 +261,7 @@ Quaternion IterateIK3D::get_joint_limitation_space(int p_index, int p_joint, con void IterateIK3D::set_joint_limitation(int p_index, int p_joint, const Ref &p_limitation) { ERR_FAIL_INDEX(p_index, (int)settings.size()); - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); _unbind_joint_limitation(p_index, p_joint); joint_settings[p_joint]->limitation = p_limitation; @@ -277,7 +279,7 @@ Ref IterateIK3D::get_joint_limitation(int p_index, int p_join void IterateIK3D::set_joint_limitation_right_axis(int p_index, int p_joint, SecondaryDirection p_direction) { ERR_FAIL_INDEX(p_index, (int)settings.size()); - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); joint_settings[p_joint]->limitation_right_axis = p_direction; notify_property_list_changed(); @@ -293,7 +295,7 @@ IKModifier3D::SecondaryDirection IterateIK3D::get_joint_limitation_right_axis(in void IterateIK3D::set_joint_limitation_right_axis_vector(int p_index, int p_joint, const Vector3 &p_vector) { ERR_FAIL_INDEX(p_index, (int)settings.size()); - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); joint_settings[p_joint]->limitation_right_axis_vector = p_vector; _update_joint_limitation(p_index, p_joint); @@ -308,7 +310,7 @@ Vector3 IterateIK3D::get_joint_limitation_right_axis_vector(int p_index, int p_j void IterateIK3D::set_joint_limitation_rotation_offset(int p_index, int p_joint, const Quaternion &p_offset) { ERR_FAIL_INDEX(p_index, (int)settings.size()); - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); joint_settings[p_joint]->limitation_rotation_offset = p_offset; _update_joint_limitation(p_index, p_joint); @@ -350,7 +352,7 @@ void IterateIK3D::_validate_axis(Skeleton3D *p_skeleton, int p_index, int p_join if (p_joint < (int)iterate_settings[p_index]->joints.size() - 1) { fwd = p_skeleton->get_bone_rest(iterate_settings[p_index]->joints[p_joint + 1].bone).origin; } else if (iterate_settings[p_index]->extend_end_bone) { - fwd = get_bone_axis(iterate_settings[p_index]->end_bone.bone, iterate_settings[p_index]->end_bone_direction); + fwd = IKModifier3D::get_bone_axis(p_skeleton, iterate_settings[p_index]->end_bone.bone, iterate_settings[p_index]->end_bone_direction, mutable_bone_axes); if (fwd.is_zero_approx()) { return; } @@ -368,6 +370,8 @@ void IterateIK3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_min_distance"), &IterateIK3D::get_min_distance); ClassDB::bind_method(D_METHOD("set_angular_delta_limit", "angular_delta_limit"), &IterateIK3D::set_angular_delta_limit); ClassDB::bind_method(D_METHOD("get_angular_delta_limit"), &IterateIK3D::get_angular_delta_limit); + ClassDB::bind_method(D_METHOD("set_deterministic", "deterministic"), &IterateIK3D::set_deterministic); + ClassDB::bind_method(D_METHOD("is_deterministic"), &IterateIK3D::is_deterministic); // Setting. ClassDB::bind_method(D_METHOD("set_target_node", "index", "target_node"), &IterateIK3D::set_target_node); @@ -390,13 +394,13 @@ void IterateIK3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "max_iterations", PROPERTY_HINT_RANGE, "0,100,or_greater"), "set_max_iterations", "get_max_iterations"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "min_distance", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater"), "set_min_distance", "get_min_distance"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "angular_delta_limit", PROPERTY_HINT_RANGE, "0,180,0.001,radians_as_degrees"), "set_angular_delta_limit", "get_angular_delta_limit"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "deterministic"), "set_deterministic", "is_deterministic"); ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/"); } -void IterateIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) { +void IterateIK3D::_clear_joints(int p_index) { IterateIK3DSetting *setting = iterate_settings[p_index]; - cached_space = p_skeleton->get_global_transform_interpolated(); - if (!setting->simulation_dirty) { + if (!setting) { return; } _unbind_joint_limitations(p_index); @@ -408,35 +412,29 @@ void IterateIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) { } setting->solver_info_list.clear(); setting->solver_info_list.resize_initialized(setting->joints.size()); - setting->chain.clear(); - bool extend_end_bone = setting->extend_end_bone && setting->end_bone_length > 0; - for (uint32_t i = 0; i < setting->joints.size(); i++) { - setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).origin); - bool last = i == setting->joints.size() - 1; - if (last && extend_end_bone && setting->end_bone_length > 0) { - Vector3 axis = get_bone_axis(setting->end_bone.bone, setting->end_bone_direction); - if (axis.is_zero_approx()) { - continue; - } - setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo); - setting->solver_info_list[i]->forward_vector = snap_vector_to_plane(setting->joint_settings[i]->get_rotation_axis_vector(), axis.normalized()); - setting->solver_info_list[i]->length = setting->end_bone_length; - setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).xform(axis * setting->end_bone_length)); - } else if (!last) { - Vector3 axis = p_skeleton->get_bone_rest(setting->joints[i + 1].bone).origin; - if (axis.is_zero_approx()) { - continue; // Means always we need to check solver info, but `!solver_info` means that the bone is zero length, so IK should skip it in the all process. - } - setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo); - setting->solver_info_list[i]->forward_vector = snap_vector_to_plane(setting->joint_settings[i]->get_rotation_axis_vector(), axis.normalized()); - setting->solver_info_list[i]->length = axis.length(); - } - } _bind_joint_limitations(p_index); +} - setting->init_current_joint_rotations(p_skeleton); +void IterateIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) { + IterateIK3DSetting *setting = iterate_settings[p_index]; + if (!setting) { + return; + } + cached_space = p_skeleton->get_global_transform_interpolated(); + if (setting->simulation_dirty) { + _clear_joints(p_index); + setting->init_joints(p_skeleton, mutable_bone_axes); + setting->simulation_dirty = false; + } else if (deterministic) { + setting->init_joints(p_skeleton, mutable_bone_axes); + } - setting->simulation_dirty = false; + if (mutable_bone_axes) { +#ifdef TOOLS_ENABLED + _update_mutable_info(); +#endif // TOOLS_ENABLED + _update_bone_axis(p_skeleton, p_index); + } setting->simulated = false; } @@ -446,6 +444,63 @@ void IterateIK3D::_make_simulation_dirty(int p_index) { return; } setting->simulation_dirty = true; +#ifdef TOOLS_ENABLED + if (!mutable_bone_axes) { + _make_gizmo_dirty(); + } +#endif // TOOLS_ENABLED +} + +void IterateIK3D::_update_bone_axis(Skeleton3D *p_skeleton, int p_index) { +#ifdef TOOLS_ENABLED + bool changed = false; +#endif // TOOLS_ENABLED + IterateIK3DSetting *setting = iterate_settings[p_index]; + const LocalVector &joints = setting->joints; + const LocalVector &solver_info_list = setting->solver_info_list; + int len = (int)solver_info_list.size() - 1; + for (int j = 0; j < len; j++) { + IterateIK3DJointSetting *joint_setting = setting->joint_settings[j]; + if (!joint_setting || !solver_info_list[j]) { + continue; + } + Vector3 axis = p_skeleton->get_bone_pose(joints[j + 1].bone).origin; + if (axis.is_zero_approx()) { + continue; + } + // Less computing. +#ifdef TOOLS_ENABLED + if (!changed) { + Vector3 old_v = solver_info_list[j]->forward_vector; + solver_info_list[j]->forward_vector = snap_vector_to_plane(joint_setting->get_rotation_axis_vector(), axis.normalized()); + changed = changed || !old_v.is_equal_approx(solver_info_list[j]->forward_vector); + float old_l = solver_info_list[j]->length; + solver_info_list[j]->length = axis.length(); + changed = changed || !Math::is_equal_approx(old_l, solver_info_list[j]->length); + } else { + solver_info_list[j]->forward_vector = snap_vector_to_plane(joint_setting->get_rotation_axis_vector(), axis.normalized()); + solver_info_list[j]->length = axis.length(); + } +#else + solver_info_list[j]->forward_vector = snap_vector_to_plane(joint_setting->get_rotation_axis_vector(), axis.normalized()); + solver_info_list[j]->length = axis.length(); +#endif // TOOLS_ENABLED + } + if (setting->extend_end_bone && len >= 0) { + IterateIK3DJointSetting *joint_setting = setting->joint_settings[len]; + if (joint_setting && solver_info_list[len]) { + Vector3 axis = IKModifier3D::get_bone_axis(p_skeleton, setting->end_bone.bone, setting->end_bone_direction, mutable_bone_axes); + if (!axis.is_zero_approx()) { + solver_info_list[len]->forward_vector = snap_vector_to_plane(joint_setting->get_rotation_axis_vector(), axis.normalized()); + solver_info_list[len]->length = setting->end_bone_length; + } + } + } +#ifdef TOOLS_ENABLED + if (changed) { + _make_gizmo_dirty(); + } +#endif // TOOLS_ENABLED } void IterateIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) { @@ -505,7 +560,7 @@ void IterateIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, Itera void IterateIK3D::_update_joint_limitation(int p_index, int p_joint) { ERR_FAIL_INDEX(p_index, (int)iterate_settings.size()); iterate_settings[p_index]->simulated = false; - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); // p_joint is unused directly, but need to identify bound index. #ifdef TOOLS_ENABLED update_gizmos(); @@ -514,7 +569,7 @@ void IterateIK3D::_update_joint_limitation(int p_index, int p_joint) { void IterateIK3D::_bind_joint_limitation(int p_index, int p_joint) { ERR_FAIL_INDEX(p_index, (int)iterate_settings.size()); - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); if (joint_settings[p_joint]->limitation.is_valid()) { joint_settings[p_joint]->limitation->connect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, p_joint)); @@ -523,7 +578,7 @@ void IterateIK3D::_bind_joint_limitation(int p_index, int p_joint) { void IterateIK3D::_unbind_joint_limitation(int p_index, int p_joint) { ERR_FAIL_INDEX(p_index, (int)iterate_settings.size()); - LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; + const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings; ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); if (joint_settings[p_joint]->limitation.is_valid()) { joint_settings[p_joint]->limitation->disconnect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, p_joint)); @@ -546,6 +601,30 @@ void IterateIK3D::_unbind_joint_limitations(int p_index) { } } +#ifdef TOOLS_ENABLED +Vector3 IterateIK3D::get_bone_vector(int p_index, int p_joint) const { + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return Vector3(); + } + ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3()); + IterateIK3DSetting *setting = iterate_settings[p_index]; + if (!setting) { + return Vector3(); + } + const LocalVector &joints = setting->joints; + ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), Vector3()); + const LocalVector &solver_info_list = setting->solver_info_list; + if (p_joint >= (int)solver_info_list.size() || !solver_info_list[p_joint]) { + if (p_joint == (int)joints.size() - 1) { + return IKModifier3D::get_bone_axis(skeleton, setting->end_bone.bone, setting->end_bone_direction, mutable_bone_axes) * setting->end_bone_length; + } + return mutable_bone_axes ? skeleton->get_bone_pose(joints[p_joint + 1].bone).origin : skeleton->get_bone_rest(joints[p_joint + 1].bone).origin; + } + return solver_info_list[p_joint]->forward_vector * solver_info_list[p_joint]->length; +} +#endif // TOOLS_ENABLED + IterateIK3D::~IterateIK3D() { for (uint32_t i = 0; i < iterate_settings.size(); i++) { _unbind_joint_limitations(i); diff --git a/scene/3d/iterate_ik_3d.h b/scene/3d/iterate_ik_3d.h index 0bb917da45c2..ea7f5a58c0b6 100644 --- a/scene/3d/iterate_ik_3d.h +++ b/scene/3d/iterate_ik_3d.h @@ -228,6 +228,38 @@ class IterateIK3D : public ChainIK3D { cache_current_vectors(p_skeleton); } + void init_joints(Skeleton3D *p_skeleton, bool p_mutable_bone_axes) { + chain.clear(); + bool extends_end = extend_end_bone && end_bone_length > 0; + for (uint32_t i = 0; i < joints.size(); i++) { + chain.push_back(p_skeleton->get_bone_global_pose(joints[i].bone).origin); + bool last = i == joints.size() - 1; + if (last && extends_end) { + Vector3 axis = IKModifier3D::get_bone_axis(p_skeleton, end_bone.bone, end_bone_direction, p_mutable_bone_axes); + if (axis.is_zero_approx()) { + continue; + } + if (!solver_info_list[i]) { + solver_info_list[i] = memnew(IKModifier3DSolverInfo); + } + solver_info_list[i]->forward_vector = snap_vector_to_plane(joint_settings[i]->get_rotation_axis_vector(), axis.normalized()); + solver_info_list[i]->length = end_bone_length; + chain.push_back(p_skeleton->get_bone_global_pose(joints[i].bone).xform(axis * end_bone_length)); + } else if (!last) { + Vector3 axis = p_skeleton->get_bone_rest(joints[i + 1].bone).origin; + if (axis.is_zero_approx()) { + continue; + } + if (!solver_info_list[i]) { + solver_info_list[i] = memnew(IKModifier3DSolverInfo); + } + solver_info_list[i]->forward_vector = snap_vector_to_plane(joint_settings[i]->get_rotation_axis_vector(), axis.normalized()); + solver_info_list[i]->length = axis.length(); + } + } + init_current_joint_rotations(p_skeleton); + } + ~IterateIK3DSetting() { for (uint32_t i = 0; i < joint_settings.size(); i++) { if (joint_settings[i]) { @@ -247,6 +279,8 @@ class IterateIK3D : public ChainIK3D { double min_distance_squared = min_distance * min_distance; // For cache. double angular_delta_limit = Math::deg_to_rad(2.0); // If the delta is too large, the results before and after iterating can change significantly, and divergence of calculations can easily occur. + bool deterministic = false; + bool _get(const StringName &p_path, Variant &r_ret) const; bool _set(const StringName &p_path, const Variant &p_value); void _get_property_list(List *p_list) const; @@ -256,7 +290,10 @@ class IterateIK3D : public ChainIK3D { virtual void _validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const override; virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override; + void _clear_joints(int p_index); // Connect signal with the IterateIK3D node so it shouldn't be included by struct IterateIK3DSetting. + virtual void _make_simulation_dirty(int p_index) override; + virtual void _update_bone_axis(Skeleton3D *p_skeleton, int p_index) override; virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override; void _process_joints(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_target_destination); @@ -290,6 +327,9 @@ class IterateIK3D : public ChainIK3D { void set_angular_delta_limit(double p_angular_delta_limit); double get_angular_delta_limit() const; + void set_deterministic(bool p_deterministic); + bool is_deterministic() const; + // Setting. void set_target_node(int p_index, const NodePath &p_target_node); NodePath get_target_node(int p_index) const; @@ -311,5 +351,9 @@ class IterateIK3D : public ChainIK3D { // Helper. Quaternion get_joint_limitation_space(int p_index, int p_joint, const Vector3 &p_forward) const; +#ifdef TOOLS_ENABLED + virtual Vector3 get_bone_vector(int p_index, int p_joint) const override; +#endif // TOOLS_ENABLED + ~IterateIK3D(); }; diff --git a/scene/3d/spline_ik_3d.cpp b/scene/3d/spline_ik_3d.cpp index 9e02620f09e3..0e3119c448ff 100644 --- a/scene/3d/spline_ik_3d.cpp +++ b/scene/3d/spline_ik_3d.cpp @@ -189,6 +189,9 @@ void SplineIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) { SplineIK3DSetting *setting = sp_settings[p_index]; cached_space = p_skeleton->get_global_transform_interpolated(); if (!setting->simulation_dirty) { + if (mutable_bone_axes) { + _update_bone_axis(p_skeleton, p_index); + } return; } for (uint32_t i = 0; i < setting->solver_info_list.size(); i++) { @@ -205,7 +208,7 @@ void SplineIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) { setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).origin); bool last = i == setting->joints.size() - 1; if (last && extend_end_bone && setting->end_bone_length > 0) { - Vector3 axis = get_bone_axis(setting->end_bone.bone, setting->end_bone_direction); + Vector3 axis = IKModifier3D::get_bone_axis(p_skeleton, setting->end_bone.bone, setting->end_bone_direction, mutable_bone_axes); if (axis.is_zero_approx()) { setting->chain_length_accum[i] = accum; continue; @@ -230,6 +233,14 @@ void SplineIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) { setting->chain_length_accum[i] = accum; } + if (mutable_bone_axes) { + _update_bone_axis(p_skeleton, p_index); +#ifdef TOOLS_ENABLED + } else { + _make_gizmo_dirty(); +#endif // TOOLS_ENABLED + } + setting->init_current_joint_rotations(p_skeleton); setting->simulation_dirty = false; @@ -243,6 +254,56 @@ void SplineIK3D::_make_simulation_dirty(int p_index) { setting->simulation_dirty = true; } +void SplineIK3D::_update_bone_axis(Skeleton3D *p_skeleton, int p_index) { +#ifdef TOOLS_ENABLED + bool changed = false; +#endif // TOOLS_ENABLED + SplineIK3DSetting *setting = sp_settings[p_index]; + const LocalVector &joints = setting->joints; + const LocalVector &solver_info_list = setting->solver_info_list; + int len = (int)solver_info_list.size() - 1; + for (int j = 0; j < len; j++) { + if (!solver_info_list[j]) { + continue; + } + Vector3 axis = p_skeleton->get_bone_pose(joints[j + 1].bone).origin; + if (axis.is_zero_approx()) { + continue; + } + // Less computing. +#ifdef TOOLS_ENABLED + if (!changed) { + Vector3 old_v = solver_info_list[j]->forward_vector; + solver_info_list[j]->forward_vector = axis.normalized(); + changed = changed || !old_v.is_equal_approx(solver_info_list[j]->forward_vector); + float old_l = solver_info_list[j]->length; + solver_info_list[j]->length = axis.length(); + changed = changed || !Math::is_equal_approx(old_l, solver_info_list[j]->length); + } else { + solver_info_list[j]->forward_vector = axis.normalized(); + solver_info_list[j]->length = axis.length(); + } +#else + solver_info_list[j]->forward_vector = axis.normalized(); + solver_info_list[j]->length = axis.length(); +#endif // TOOLS_ENABLED + } + if (setting->extend_end_bone && len >= 0) { + if (solver_info_list[len]) { + Vector3 axis = IKModifier3D::get_bone_axis(p_skeleton, setting->end_bone.bone, setting->end_bone_direction, mutable_bone_axes); + if (!axis.is_zero_approx()) { + solver_info_list[len]->forward_vector = axis.normalized(); + solver_info_list[len]->length = setting->end_bone_length; + } + } + } +#ifdef TOOLS_ENABLED + if (changed) { + _make_gizmo_dirty(); + } +#endif // TOOLS_ENABLED +} + void SplineIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) { for (uint32_t i = 0; i < settings.size(); i++) { _init_joints(p_skeleton, i); @@ -430,6 +491,30 @@ void SplineIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, SplineI } } +#ifdef TOOLS_ENABLED +Vector3 SplineIK3D::get_bone_vector(int p_index, int p_joint) const { + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return Vector3(); + } + ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3()); + SplineIK3DSetting *setting = sp_settings[p_index]; + if (!setting) { + return Vector3(); + } + const LocalVector &joints = setting->joints; + ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), Vector3()); + const LocalVector &solver_info_list = setting->solver_info_list; + if (p_joint >= (int)solver_info_list.size() || !solver_info_list[p_joint]) { + if (p_joint == (int)joints.size() - 1) { + return IKModifier3D::get_bone_axis(skeleton, setting->end_bone.bone, setting->end_bone_direction, mutable_bone_axes) * setting->end_bone_length; + } + return mutable_bone_axes ? skeleton->get_bone_pose(joints[p_joint + 1].bone).origin : skeleton->get_bone_rest(joints[p_joint + 1].bone).origin; + } + return solver_info_list[p_joint]->forward_vector * solver_info_list[p_joint]->length; +} +#endif // TOOLS_ENABLED + SplineIK3D::~SplineIK3D() { clear_settings(); } diff --git a/scene/3d/spline_ik_3d.h b/scene/3d/spline_ik_3d.h index cd46b58e03b6..4ac2985037eb 100644 --- a/scene/3d/spline_ik_3d.h +++ b/scene/3d/spline_ik_3d.h @@ -151,6 +151,7 @@ class SplineIK3D : public ChainIK3D { virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override; virtual void _make_simulation_dirty(int p_index) override; + virtual void _update_bone_axis(Skeleton3D *p_skeleton, int p_index) override; virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override; void _process_joints(double p_delta, Skeleton3D *p_skeleton, SplineIK3DSetting *p_setting, Ref p_curve, const Transform3D &p_curve_space); @@ -183,5 +184,9 @@ class SplineIK3D : public ChainIK3D { // Helper. double get_bezier_arc_length(); +#ifdef TOOLS_ENABLED + virtual Vector3 get_bone_vector(int p_index, int p_joint) const override; +#endif // TOOLS_ENABLED + ~SplineIK3D(); }; diff --git a/scene/3d/spring_bone_simulator_3d.cpp b/scene/3d/spring_bone_simulator_3d.cpp index d84877d370ec..430b899062a5 100644 --- a/scene/3d/spring_bone_simulator_3d.cpp +++ b/scene/3d/spring_bone_simulator_3d.cpp @@ -427,7 +427,7 @@ void SpringBoneSimulator3D::_notification(int p_what) { } break; #ifdef TOOLS_ENABLED case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: { - update_gizmos(); + _make_gizmo_dirty(); } break; case NOTIFICATION_EDITOR_PRE_SAVE: { saving = true; @@ -531,6 +531,9 @@ bool SpringBoneSimulator3D::is_end_bone_extended(int p_index) const { void SpringBoneSimulator3D::set_end_bone_direction(int p_index, BoneDirection p_bone_direction) { ERR_FAIL_INDEX(p_index, (int)settings.size()); settings[p_index]->end_bone_direction = p_bone_direction; + if (mutable_bone_axes) { + return; // Chain dir will be recaluclated in _update_bone_axis(). + } _make_joints_dirty(p_index); } @@ -541,7 +544,11 @@ SkeletonModifier3D::BoneDirection SpringBoneSimulator3D::get_end_bone_direction( void SpringBoneSimulator3D::set_end_bone_length(int p_index, float p_length) { ERR_FAIL_INDEX(p_index, (int)settings.size()); + float old = settings[p_index]->end_bone_length; settings[p_index]->end_bone_length = p_length; + if (mutable_bone_axes && Math::is_zero_approx(old) == Math::is_zero_approx(p_length)) { + return; // If chain size is not changed, length will be recaluclated in _update_bone_axis(). + } _make_joints_dirty(p_index); } @@ -555,7 +562,7 @@ Vector3 SpringBoneSimulator3D::get_end_bone_axis(int p_end_bone, BoneDirection p if (p_direction == BONE_DIRECTION_FROM_PARENT) { Skeleton3D *sk = get_skeleton(); if (sk) { - axis = sk->get_bone_rest(p_end_bone).basis.xform_inv(sk->get_bone_rest(p_end_bone).origin); + axis = sk->get_bone_rest(p_end_bone).basis.xform_inv(mutable_bone_axes ? sk->get_bone_pose(p_end_bone).origin : sk->get_bone_rest(p_end_bone).origin); axis.normalize(); } } else { @@ -873,7 +880,7 @@ bool SpringBoneSimulator3D::is_config_individual(int p_index) const { void SpringBoneSimulator3D::set_joint_bone_name(int p_index, int p_joint, const String &p_bone_name) { // Exists only for indicate bone name on the inspector, no needs to make dirty joint array. ERR_FAIL_INDEX(p_index, (int)settings.size()); - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->bone_name = p_bone_name; Skeleton3D *sk = get_skeleton(); @@ -891,7 +898,7 @@ String SpringBoneSimulator3D::get_joint_bone_name(int p_index, int p_joint) cons void SpringBoneSimulator3D::set_joint_bone(int p_index, int p_joint, int p_bone) { ERR_FAIL_INDEX(p_index, (int)settings.size()); - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->bone = p_bone; Skeleton3D *sk = get_skeleton(); @@ -917,11 +924,11 @@ void SpringBoneSimulator3D::set_joint_radius(int p_index, int p_joint, float p_r if (!is_config_individual(p_index)) { return; // Joints are read-only. } - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->radius = p_radius; #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -937,7 +944,7 @@ void SpringBoneSimulator3D::set_joint_stiffness(int p_index, int p_joint, float if (!is_config_individual(p_index)) { return; // Joints are read-only. } - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->stiffness = p_stiffness; } @@ -954,7 +961,7 @@ void SpringBoneSimulator3D::set_joint_drag(int p_index, int p_joint, float p_dra if (!is_config_individual(p_index)) { return; // Joints are read-only. } - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->drag = p_drag; } @@ -971,7 +978,7 @@ void SpringBoneSimulator3D::set_joint_gravity(int p_index, int p_joint, float p_ if (!is_config_individual(p_index)) { return; // Joints are read-only. } - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->gravity = p_gravity; } @@ -989,7 +996,7 @@ void SpringBoneSimulator3D::set_joint_gravity_direction(int p_index, int p_joint if (!is_config_individual(p_index)) { return; // Joints are read-only. } - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->gravity_direction = p_gravity_direction; } @@ -1006,7 +1013,7 @@ void SpringBoneSimulator3D::set_joint_rotation_axis(int p_index, int p_joint, Ro if (!is_config_individual(p_index)) { return; // Joints are read-only. } - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->rotation_axis = p_axis; Skeleton3D *sk = get_skeleton(); @@ -1016,7 +1023,7 @@ void SpringBoneSimulator3D::set_joint_rotation_axis(int p_index, int p_joint, Ro notify_property_list_changed(); settings[p_index]->simulation_dirty = true; #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -1032,7 +1039,7 @@ void SpringBoneSimulator3D::set_joint_rotation_axis_vector(int p_index, int p_jo if (!is_config_individual(p_index) || settings[p_index]->rotation_axis != ROTATION_AXIS_CUSTOM) { return; // Joints are read-only. } - LocalVector &joints = settings[p_index]->joints; + const LocalVector &joints = settings[p_index]->joints; ERR_FAIL_INDEX(p_joint, (int)joints.size()); joints[p_joint]->rotation_axis_vector = p_vector; Skeleton3D *sk = get_skeleton(); @@ -1041,7 +1048,7 @@ void SpringBoneSimulator3D::set_joint_rotation_axis_vector(int p_index, int p_jo } settings[p_index]->simulation_dirty = true; #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -1220,6 +1227,17 @@ Vector3 SpringBoneSimulator3D::get_external_force() const { return external_force; } +void SpringBoneSimulator3D::set_mutable_bone_axes(bool p_enabled) { + mutable_bone_axes = p_enabled; + for (SpringBone3DSetting *setting : settings) { + setting->simulation_dirty = true; + } +} + +bool SpringBoneSimulator3D::are_bone_axes_mutable() const { + return mutable_bone_axes; +} + void SpringBoneSimulator3D::_bind_methods() { // Setting. ClassDB::bind_method(D_METHOD("set_root_bone_name", "index", "bone_name"), &SpringBoneSimulator3D::set_root_bone_name); @@ -1319,10 +1337,14 @@ void SpringBoneSimulator3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_external_force", "force"), &SpringBoneSimulator3D::set_external_force); ClassDB::bind_method(D_METHOD("get_external_force"), &SpringBoneSimulator3D::get_external_force); + ClassDB::bind_method(D_METHOD("set_mutable_bone_axes", "enabled"), &SpringBoneSimulator3D::set_mutable_bone_axes); + ClassDB::bind_method(D_METHOD("are_bone_axes_mutable"), &SpringBoneSimulator3D::are_bone_axes_mutable); + // To process manually. ClassDB::bind_method(D_METHOD("reset"), &SpringBoneSimulator3D::reset); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "external_force", PROPERTY_HINT_RANGE, "-99999,99999,or_greater,or_less,hide_control,suffix:m/s"), "set_external_force", "get_external_force"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "mutable_bone_axes"), "set_mutable_bone_axes", "are_bone_axes_mutable"); ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/"); BIND_ENUM_CONSTANT(CENTER_FROM_WORLD_ORIGIN); @@ -1601,10 +1623,94 @@ void SpringBoneSimulator3D::_update_joints() { _validate_rotation_axes(sk); } #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); +#endif // TOOLS_ENABLED +} + +void SpringBoneSimulator3D::_update_bone_axis(Skeleton3D *p_skeleton, SpringBone3DSetting *p_setting) { +#ifdef TOOLS_ENABLED + bool changed = false; +#endif // TOOLS_ENABLED + const LocalVector &joints = p_setting->joints; + int len = (int)joints.size() - 1; + for (int j = 0; j < len; j++) { + if (!joints[j]->verlet) { + continue; + } + Vector3 axis = p_skeleton->get_bone_pose(joints[j + 1]->bone).origin; + if (axis.is_zero_approx()) { + continue; + } + // Less computing. +#ifdef TOOLS_ENABLED + if (!changed) { + Vector3 old_v = joints[j]->verlet->forward_vector; + joints[j]->verlet->forward_vector = snap_vector_to_plane(joints[j]->get_rotation_axis_vector(), axis.normalized()); + changed = changed || !old_v.is_equal_approx(joints[j]->verlet->forward_vector); + float old_l = joints[j]->verlet->length; + joints[j]->verlet->length = axis.length(); + changed = changed || !Math::is_equal_approx(old_l, joints[j]->verlet->length); + } else { + joints[j]->verlet->forward_vector = snap_vector_to_plane(joints[j]->get_rotation_axis_vector(), axis.normalized()); + joints[j]->verlet->length = axis.length(); + } +#else + joints[j]->verlet->forward_vector = snap_vector_to_plane(joints[j]->get_rotation_axis_vector(), axis.normalized()); + joints[j]->verlet->length = axis.length(); +#endif // TOOLS_ENABLED + } + if (p_setting->extend_end_bone && len >= 0) { + if (joints[len]->verlet) { + Vector3 axis = get_end_bone_axis(p_setting->end_bone, p_setting->end_bone_direction); + if (!axis.is_zero_approx()) { + joints[len]->verlet->forward_vector = snap_vector_to_plane(joints[len]->get_rotation_axis_vector(), axis.normalized()); + joints[len]->verlet->length = p_setting->end_bone_length; + } + } + } +#ifdef TOOLS_ENABLED + if (changed) { + _make_gizmo_dirty(); + } #endif // TOOLS_ENABLED } +#ifdef TOOLS_ENABLED +Vector3 SpringBoneSimulator3D::get_bone_vector(int p_index, int p_joint) const { + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return Vector3(); + } + ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3()); + SpringBone3DSetting *setting = settings[p_index]; + if (!setting) { + return Vector3(); + } + const LocalVector &joints = setting->joints; + ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), Vector3()); + if (!joints[p_joint]->verlet) { + if (p_joint == (int)joints.size() - 1) { + return get_end_bone_axis(setting->end_bone, setting->end_bone_direction) * setting->end_bone_length; + } + return mutable_bone_axes ? skeleton->get_bone_pose(joints[p_joint + 1]->bone).origin : skeleton->get_bone_rest(joints[p_joint + 1]->bone).origin; + } + return joints[p_joint]->verlet->forward_vector * joints[p_joint]->verlet->length; +} + +void SpringBoneSimulator3D::_make_gizmo_dirty() { + if (gizmo_dirty) { + return; + } + gizmo_dirty = true; + callable_mp(this, &SpringBoneSimulator3D::_redraw_gizmo).call_deferred(); +} + +void SpringBoneSimulator3D::_redraw_gizmo() { + update_gizmos(); + gizmo_dirty = false; +} +#endif + void SpringBoneSimulator3D::_set_active(bool p_active) { if (p_active) { reset(); @@ -1671,6 +1777,9 @@ void SpringBoneSimulator3D::_init_joints(Skeleton3D *p_skeleton, SpringBone3DSet setting->cached_inverted_center = setting->cached_center.affine_inverse(); if (!setting->simulation_dirty) { + if (mutable_bone_axes) { + _update_bone_axis(p_skeleton, setting); + } return; } for (uint32_t i = 0; i < setting->joints.size(); i++) { @@ -1679,8 +1788,11 @@ void SpringBoneSimulator3D::_init_joints(Skeleton3D *p_skeleton, SpringBone3DSet setting->joints[i]->verlet = nullptr; } if (i < setting->joints.size() - 1) { - setting->joints[i]->verlet = memnew(SpringBone3DVerletInfo); Vector3 axis = p_skeleton->get_bone_rest(setting->joints[i + 1]->bone).origin; + if (axis.is_zero_approx()) { + continue; + } + setting->joints[i]->verlet = memnew(SpringBone3DVerletInfo); setting->joints[i]->verlet->current_tail = setting->cached_center.xform(p_skeleton->get_bone_global_pose(setting->joints[i]->bone).xform(axis)); setting->joints[i]->verlet->prev_tail = setting->joints[i]->verlet->current_tail; setting->joints[i]->verlet->forward_vector = snap_vector_to_plane(setting->joints[i]->get_rotation_axis_vector(), axis.normalized()); @@ -1699,6 +1811,13 @@ void SpringBoneSimulator3D::_init_joints(Skeleton3D *p_skeleton, SpringBone3DSet setting->joints[i]->verlet->current_rot = Quaternion(0, 0, 0, 1); } } + if (mutable_bone_axes) { + _update_bone_axis(p_skeleton, setting); +#ifdef TOOLS_ENABLED + } else { + _make_gizmo_dirty(); +#endif // TOOLS_ENABLED + } setting->simulation_dirty = false; } diff --git a/scene/3d/spring_bone_simulator_3d.h b/scene/3d/spring_bone_simulator_3d.h index e5fe2296ca3d..421d5f8089f4 100644 --- a/scene/3d/spring_bone_simulator_3d.h +++ b/scene/3d/spring_bone_simulator_3d.h @@ -64,8 +64,8 @@ class SpringBoneSimulator3D : public SkeletonModifier3D { struct SpringBone3DVerletInfo { Vector3 prev_tail; Vector3 current_tail; - Vector3 forward_vector; Quaternion current_rot; + Vector3 forward_vector; float length = 0.0; }; @@ -156,6 +156,7 @@ class SpringBoneSimulator3D : public SkeletonModifier3D { protected: LocalVector settings; Vector3 external_force; + bool mutable_bone_axes = true; bool _get(const StringName &p_path, Variant &r_ret) const; bool _set(const StringName &p_path, const Variant &p_value); @@ -180,6 +181,14 @@ class SpringBoneSimulator3D : public SkeletonModifier3D { void _update_joint_array(int p_index); void _update_joints(); + void _update_bone_axis(Skeleton3D *p_skeleton, SpringBone3DSetting *p_setting); + +#ifdef TOOLS_ENABLED + bool gizmo_dirty = false; + void _make_gizmo_dirty(); + void _redraw_gizmo(); +#endif // TOOLS_ENABLED + virtual void add_child_notify(Node *p_child) override; virtual void move_child_notify(Node *p_child) override; virtual void remove_child_notify(Node *p_child) override; @@ -304,12 +313,16 @@ class SpringBoneSimulator3D : public SkeletonModifier3D { void set_external_force(const Vector3 &p_force); Vector3 get_external_force() const; + void set_mutable_bone_axes(bool p_enabled); + bool are_bone_axes_mutable() const; + // To process manually. void reset(); #ifdef TOOLS_ENABLED + Vector3 get_bone_vector(int p_index, int p_joint) const; virtual bool is_processed_on_saving() const override { return true; } -#endif +#endif // TOOLS_ENABLED ~SpringBoneSimulator3D(); }; diff --git a/scene/3d/two_bone_ik_3d.cpp b/scene/3d/two_bone_ik_3d.cpp index 1a6b2bc2abdc..60a0cffdb057 100644 --- a/scene/3d/two_bone_ik_3d.cpp +++ b/scene/3d/two_bone_ik_3d.cpp @@ -352,7 +352,7 @@ void TwoBoneIK3D::set_extend_end_bone(int p_index, bool p_enabled) { } notify_property_list_changed(); #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -364,13 +364,16 @@ bool TwoBoneIK3D::is_end_bone_extended(int p_index) const { void TwoBoneIK3D::set_end_bone_direction(int p_index, BoneDirection p_bone_direction) { ERR_FAIL_INDEX(p_index, (int)settings.size()); tb_settings[p_index]->end_bone_direction = p_bone_direction; - tb_settings[p_index]->simulation_dirty = true; Skeleton3D *sk = get_skeleton(); if (sk) { _validate_pole_direction(sk, p_index); } + if (mutable_bone_axes) { + return; // Chain dir will be recaluclated in _update_bone_axis(). + } + tb_settings[p_index]->simulation_dirty = true; #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -381,10 +384,14 @@ SkeletonModifier3D::BoneDirection TwoBoneIK3D::get_end_bone_direction(int p_inde void TwoBoneIK3D::set_end_bone_length(int p_index, float p_length) { ERR_FAIL_INDEX(p_index, (int)settings.size()); + float old = tb_settings[p_index]->end_bone_length; tb_settings[p_index]->end_bone_length = p_length; + if (mutable_bone_axes && Math::is_zero_approx(old) == Math::is_zero_approx(p_length)) { + return; // If chain size is not changed, length will be recaluclated in _update_bone_axis(). + } tb_settings[p_index]->simulation_dirty = true; #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -425,7 +432,7 @@ void TwoBoneIK3D::set_pole_direction(int p_index, SecondaryDirection p_direction } notify_property_list_changed(); #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -446,7 +453,7 @@ void TwoBoneIK3D::set_pole_direction_vector(int p_index, const Vector3 &p_vector _validate_pole_direction(sk, p_index); } #ifdef TOOLS_ENABLED - update_gizmos(); + _make_gizmo_dirty(); #endif // TOOLS_ENABLED } @@ -539,7 +546,7 @@ void TwoBoneIK3D::_validate_pole_direction(Skeleton3D *p_skeleton, int p_index) // End bone. int valid_end_bone = setting->get_end_bone(); - Vector3 axis = get_bone_axis(valid_end_bone, setting->end_bone_direction); + Vector3 axis = IKModifier3D::get_bone_axis(p_skeleton, valid_end_bone, setting->end_bone_direction, mutable_bone_axes); Vector3 global_rest_origin; if (setting->extend_end_bone && setting->end_bone_length > 0 && !axis.is_zero_approx()) { global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).xform(axis * setting->end_bone_length); @@ -571,6 +578,27 @@ void TwoBoneIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) { TwoBoneIK3DSetting *setting = tb_settings[p_index]; cached_space = p_skeleton->get_global_transform_interpolated(); if (!setting->simulation_dirty) { + if (mutable_bone_axes) { + _update_bone_axis(p_skeleton, p_index); + } + return; + } + _clear_joints(p_index); + if (setting->root_bone.bone == -1 || setting->middle_bone.bone == -1 || !setting->is_end_valid()) { + return; + } + setting->mid_joint_solver_info = memnew(IKModifier3DSolverInfo); + setting->root_joint_solver_info = memnew(IKModifier3DSolverInfo); + _update_bone_axis(p_skeleton, p_index); + + setting->init_current_joint_rotations(p_skeleton); + + setting->simulation_dirty = false; +} + +void TwoBoneIK3D::_clear_joints(int p_index) { + TwoBoneIK3DSetting *setting = tb_settings[p_index]; + if (!setting) { return; } setting->root_pos = Vector3(); @@ -584,62 +612,75 @@ void TwoBoneIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) { memdelete(setting->mid_joint_solver_info); setting->mid_joint_solver_info = nullptr; } - if (setting->root_bone.bone == -1 || setting->middle_bone.bone == -1 || !setting->is_end_valid()) { +} + +void TwoBoneIK3D::_update_bone_axis(Skeleton3D *p_skeleton, int p_index) { + TwoBoneIK3DSetting *setting = tb_settings[p_index]; + if (!setting || !setting->mid_joint_solver_info || !setting->root_joint_solver_info) { return; } +#ifdef TOOLS_ENABLED + Vector3 old_mv = setting->mid_joint_solver_info->forward_vector; + float old_ml = setting->mid_joint_solver_info->length; + Vector3 old_rv = setting->root_joint_solver_info->forward_vector; + float old_rl = setting->root_joint_solver_info->length; +#endif // TOOLS_ENABLED // End bone. int valid_end_bone = setting->get_end_bone(); - Vector3 axis = get_bone_axis(valid_end_bone, setting->end_bone_direction); + Vector3 axis = IKModifier3D::get_bone_axis(p_skeleton, valid_end_bone, setting->end_bone_direction, mutable_bone_axes); Vector3 global_rest_origin; if (setting->extend_end_bone && setting->end_bone_length > 0 && !axis.is_zero_approx()) { setting->end_pos = p_skeleton->get_bone_global_pose(valid_end_bone).xform(axis * setting->end_bone_length); - global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).xform(axis * setting->end_bone_length); + global_rest_origin = _get_bone_global_rest(p_skeleton, valid_end_bone, setting->root_bone.bone).xform(axis * setting->end_bone_length); } else { // Shouldn't be using virtual end. setting->end_pos = p_skeleton->get_bone_global_pose(valid_end_bone).origin; - global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).origin; + global_rest_origin = _get_bone_global_rest(p_skeleton, valid_end_bone, setting->root_bone.bone).origin; } // Middle bone. - axis = global_rest_origin - p_skeleton->get_bone_global_rest(setting->middle_bone.bone).origin; - global_rest_origin = p_skeleton->get_bone_global_rest(setting->middle_bone.bone).origin; + Vector3 orig = _get_bone_global_rest(p_skeleton, setting->middle_bone.bone, setting->root_bone.bone).origin; + axis = global_rest_origin - orig; + global_rest_origin = orig; if (!axis.is_zero_approx()) { setting->mid_pos = p_skeleton->get_bone_global_pose(setting->middle_bone.bone).origin; setting->mid_joint_solver_info = memnew(IKModifier3DSolverInfo); setting->mid_joint_solver_info->forward_vector = p_skeleton->get_bone_global_rest(setting->middle_bone.bone).basis.get_rotation_quaternion().xform_inv(axis).normalized(); setting->mid_joint_solver_info->length = axis.length(); } else { + _clear_joints(p_index); return; } // Root bone. - axis = global_rest_origin - p_skeleton->get_bone_global_rest(setting->root_bone.bone).origin; - global_rest_origin = p_skeleton->get_bone_global_rest(setting->root_bone.bone).origin; + orig = _get_bone_global_rest(p_skeleton, setting->root_bone.bone, setting->root_bone.bone).origin; + axis = global_rest_origin - orig; + global_rest_origin = orig; if (!axis.is_zero_approx()) { setting->root_pos = p_skeleton->get_bone_global_pose(setting->root_bone.bone).origin; setting->root_joint_solver_info = memnew(IKModifier3DSolverInfo); setting->root_joint_solver_info->forward_vector = p_skeleton->get_bone_global_rest(setting->root_bone.bone).basis.get_rotation_quaternion().xform_inv(axis).normalized(); setting->root_joint_solver_info->length = axis.length(); - } else if (setting->mid_joint_solver_info) { - memdelete(setting->mid_joint_solver_info); - setting->mid_joint_solver_info = nullptr; + } else { + _clear_joints(p_index); return; } - setting->init_current_joint_rotations(p_skeleton); - double total_length = setting->root_joint_solver_info->length + setting->mid_joint_solver_info->length; setting->cached_length_sq = total_length * total_length; - - setting->simulation_dirty = false; +#ifdef TOOLS_ENABLED + if (!Math::is_equal_approx(old_ml, setting->mid_joint_solver_info->length) || !Math::is_equal_approx(old_rl, setting->root_joint_solver_info->length) || !old_mv.is_equal_approx(setting->mid_joint_solver_info->forward_vector) || !old_rv.is_equal_approx(setting->root_joint_solver_info->forward_vector)) { + _make_gizmo_dirty(); + } +#endif // TOOLS_ENABLED } void TwoBoneIK3D::_update_joints(int p_index) { tb_settings[p_index]->simulation_dirty = true; #ifdef TOOLS_ENABLED - update_gizmos(); // To clear invalid setting. + _make_gizmo_dirty(); // To clear invalid setting. #endif // TOOLS_ENABLED Skeleton3D *sk = get_skeleton(); @@ -679,9 +720,29 @@ void TwoBoneIK3D::_update_joints(int p_index) { _validate_pole_directions(sk); } + if (mutable_bone_axes) { + _update_bone_axis(sk, p_index); #ifdef TOOLS_ENABLED - update_gizmos(); + } else { + _make_gizmo_dirty(); #endif // TOOLS_ENABLED + } +} + +Transform3D TwoBoneIK3D::_get_bone_global_rest(Skeleton3D *p_skeleton, int p_bone, int p_root) const { + if (!mutable_bone_axes) { + return p_skeleton->get_bone_global_rest(p_bone); + } + Transform3D tr = Transform3D(p_skeleton->get_bone_rest(p_root).basis, p_skeleton->get_bone_pose(p_root).origin); + LocalVector path; + for (int bone = p_bone; bone != p_root && bone != -1; bone = p_skeleton->get_bone_parent(bone)) { + path.push_back(bone); + } + path.reverse(); + for (int bone : path) { + tr = tr * Transform3D(p_skeleton->get_bone_rest(bone).basis, p_skeleton->get_bone_pose(bone).origin); + } + return tr; } void TwoBoneIK3D::_make_simulation_dirty(int p_index) { @@ -773,6 +834,49 @@ void TwoBoneIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, TwoBon p_skeleton->set_bone_pose_rotation(p_setting->middle_bone.bone, get_local_pose_rotation(p_skeleton, p_setting->middle_bone.bone, p_setting->mid_joint_solver_info->current_gpose)); } +#ifdef TOOLS_ENABLED +Vector3 TwoBoneIK3D::get_root_bone_vector(int p_index) const { + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return Vector3(); + } + ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3()); + TwoBoneIK3DSetting *setting = tb_settings[p_index]; + if (!setting) { + return Vector3(); + } + if (!setting->root_joint_solver_info) { + return _get_bone_global_rest(skeleton, setting->middle_bone.bone, setting->root_bone.bone).origin - _get_bone_global_rest(skeleton, setting->root_bone.bone, setting->root_bone.bone).origin; + } + return setting->root_joint_solver_info->forward_vector * setting->root_joint_solver_info->length; +} + +Vector3 TwoBoneIK3D::get_middle_bone_vector(int p_index) const { + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return Vector3(); + } + ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3()); + TwoBoneIK3DSetting *setting = tb_settings[p_index]; + if (!setting) { + return Vector3(); + } + if (!setting->mid_joint_solver_info) { + int valid_end_bone = setting->get_end_bone(); + Vector3 axis = IKModifier3D::get_bone_axis(skeleton, valid_end_bone, setting->end_bone_direction, mutable_bone_axes); + Vector3 global_rest_origin; + if (setting->extend_end_bone && setting->end_bone_length > 0 && !axis.is_zero_approx()) { + global_rest_origin = _get_bone_global_rest(skeleton, valid_end_bone, setting->root_bone.bone).xform(axis * setting->end_bone_length); + } else { + // Shouldn't be using virtual end. + global_rest_origin = _get_bone_global_rest(skeleton, valid_end_bone, setting->root_bone.bone).origin; + } + return global_rest_origin - _get_bone_global_rest(skeleton, setting->middle_bone.bone, setting->root_bone.bone).origin; + } + return setting->mid_joint_solver_info->forward_vector * setting->mid_joint_solver_info->length; +} +#endif // TOOLS_ENABLED + TwoBoneIK3D::~TwoBoneIK3D() { clear_settings(); } diff --git a/scene/3d/two_bone_ik_3d.h b/scene/3d/two_bone_ik_3d.h index e93a56378703..28dd4790d246 100644 --- a/scene/3d/two_bone_ik_3d.h +++ b/scene/3d/two_bone_ik_3d.h @@ -252,12 +252,16 @@ class TwoBoneIK3D : public IKModifier3D { virtual void _make_all_joints_dirty() override; virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override; + void _clear_joints(int p_index); virtual void _update_joints(int p_index) override; virtual void _make_simulation_dirty(int p_index) override; + virtual void _update_bone_axis(Skeleton3D *p_skeleton, int p_index) override; virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override; void _process_joints(double p_delta, Skeleton3D *p_skeleton, TwoBoneIK3DSetting *p_setting, const Vector3 &p_destination, const Vector3 &p_pole_destination); + Transform3D _get_bone_global_rest(Skeleton3D *p_skeleton, int p_bone, int p_root) const; + public: virtual PackedStringArray get_configuration_warnings() const override; virtual void set_setting_count(int p_count) override { @@ -307,5 +311,10 @@ class TwoBoneIK3D : public IKModifier3D { bool is_valid(int p_index) const; // Helper for editor and validation. +#ifdef TOOLS_ENABLED + Vector3 get_root_bone_vector(int p_index) const; + Vector3 get_middle_bone_vector(int p_index) const; +#endif // TOOLS_ENABLED + ~TwoBoneIK3D(); };