seedlingattempt/godot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add mutable_bone_axes to IKs

Browse Source
This commit is contained in:
Silc Lizard (Tokage) Renew
2025-11-15 06:17:24 +09:00
parent ef34c3d534
commit 031fd66fed
18 changed files with 760 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
doc/classes/IKModifier3D.xml
View File

@ -5,7 +5,6 @@
</brief_description>
<description>
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.
</description>
<tutorials>
</tutorials>
@ -36,4 +35,10 @@
</description>
</method>
</methods>
<members>
<member name="mutable_bone_axes" type="bool" setter="set_mutable_bone_axes" getter="are_bone_axes_mutable" default="true">
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.
</member>
</members>
</class>

4
doc/classes/SpringBoneSimulator3D.xml
View File

@ -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.
</member>
<member name="mutable_bone_axes" type="bool" setter="set_mutable_bone_axes" getter="are_bone_axes_mutable" default="true">
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.
</member>
<member name="setting_count" type="int" setter="set_setting_count" getter="get_setting_count" default="0">
The number of settings.
</member>

67
editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp
View File

@ -135,13 +135,18 @@ Ref<ArrayMesh> 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<ArrayMesh> 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<JointLimitation3D> 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<ArrayMesh> 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<JointLimitation3D> lim = it_ik->get_joint_limitation(i, j);
@ -205,12 +217,13 @@ Ref<ArrayMesh> 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<ArrayMesh> 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);
}
}
}

18
editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp
View File

@ -142,8 +142,10 @@ Ref<ArrayMesh> 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<ArrayMesh> 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<int>(bones));
surface_tool->set_weights(Vector<float>(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<int>(bones));

17
editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp
View File

@ -134,35 +134,28 @@ Ref<ArrayMesh> 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);
}

2
scene/3d/bone_constraint_3d.cpp
View File

@ -191,7 +191,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);

72
scene/3d/chain_ik_3d.cpp
View File

@ -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,14 +270,17 @@ 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);
}
#ifdef TOOLS_ENABLED
update_gizmos();
_make_gizmo_dirty();
#endif // TOOLS_ENABLED
if (mutable_bone_axes) {
return; // Chain dir will be recaluclated in _update_bone_axis().
}
_make_simulation_dirty(p_index);
}
SkeletonModifier3D::BoneDirection ChainIK3D::get_end_bone_direction(int p_index) const {
@ -287,11 +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);
#ifdef TOOLS_ENABLED
update_gizmos();
_make_gizmo_dirty();
#endif // TOOLS_ENABLED
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);
}
float ChainIK3D::get_end_bone_length(int p_index) const {
@ -472,7 +479,7 @@ void ChainIK3D::_update_joints(int p_index) {
}
#ifdef TOOLS_ENABLED
update_gizmos();
_make_gizmo_dirty();
#endif // TOOLS_ENABLED
}
@ -480,6 +487,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();
}

30
scene/3d/chain_ik_3d.h
View File

@ -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 @@ public:
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 @@ public:
};
protected:
#ifdef TOOLS_ENABLED
virtual void _update_mutable_info() override;
#endif // TOOLS_ENABLED
LocalVector<ChainIK3DSetting *> chain_settings; // For caching.
bool _get(const StringName &p_path, Variant &r_ret) const;
@ -239,5 +258,12 @@ public:
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();
};

56
scene/3d/ik_modifier_3d.cpp
View File

@ -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 {

14
scene/3d/ik_modifier_3d.h
View File

@ -40,6 +40,7 @@ protected:
bool saving = false;
#endif // TOOLS_ENABLED
bool mutable_bone_axes = true;
Transform3D cached_space;
bool joints_dirty = false;
@ -79,6 +80,14 @@ protected:
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 @@ public:
_set_setting_count<IKModifier3DSetting>(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();

160
scene/3d/iterate_ik_3d.cpp
View File

@ -207,7 +207,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<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +215,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 +227,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<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +253,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<JointLimitation3D> &p_limitation) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +271,7 @@ Ref<JointLimitation3D> 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<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +287,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<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +302,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<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +344,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;
}
@ -393,10 +387,9 @@ void IterateIK3D::_bind_methods() {
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 +401,27 @@ 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;
}
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 +431,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<BoneJoint> &joints = setting->joints;
const LocalVector<IKModifier3DSolverInfo *> &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 +547,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<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +556,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<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +565,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<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
const LocalVector<IterateIK3DJointSetting *> &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 +588,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<BoneJoint> &joints = setting->joints;
ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), Vector3());
const LocalVector<IKModifier3DSolverInfo *> &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);

39
scene/3d/iterate_ik_3d.h
View File

@ -228,6 +228,38 @@ public:
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]) {
@ -256,7 +288,10 @@ protected:
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);
@ -311,5 +346,9 @@ public:
// 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();
};

87
scene/3d/spline_ik_3d.cpp
View File

@ -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<BoneJoint> &joints = setting->joints;
const LocalVector<IKModifier3DSolverInfo *> &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<BoneJoint> &joints = setting->joints;
ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), Vector3());
const LocalVector<IKModifier3DSolverInfo *> &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();
}

5
scene/3d/spline_ik_3d.h
View File

@ -151,6 +151,7 @@ protected:
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<Curve3D> p_curve, const Transform3D &p_curve_space);
@ -183,5 +184,9 @@ public:
// 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();
};

157
scene/3d/spring_bone_simulator_3d.cpp
View File

@ -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,12 @@ 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;
#ifdef TOOLS_ENABLED
_make_gizmo_dirty();
#endif // TOOLS_ENABLED
if (mutable_bone_axes) {
return; // Chain dir will be recaluclated in _update_bone_axis().
}
_make_joints_dirty(p_index);
}
@ -541,7 +547,14 @@ 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;
#ifdef TOOLS_ENABLED
_make_gizmo_dirty();
#endif // TOOLS_ENABLED
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 +568,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 +886,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &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 +904,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &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 +930,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &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 +950,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
ERR_FAIL_INDEX(p_joint, (int)joints.size());
joints[p_joint]->stiffness = p_stiffness;
}
@ -954,7 +967,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
ERR_FAIL_INDEX(p_joint, (int)joints.size());
joints[p_joint]->drag = p_drag;
}
@ -971,7 +984,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
ERR_FAIL_INDEX(p_joint, (int)joints.size());
joints[p_joint]->gravity = p_gravity;
}
@ -989,7 +1002,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
ERR_FAIL_INDEX(p_joint, (int)joints.size());
joints[p_joint]->gravity_direction = p_gravity_direction;
}
@ -1006,7 +1019,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &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 +1029,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 +1045,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<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
const LocalVector<SpringBone3DJointSetting *> &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 +1054,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 +1233,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 +1343,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 +1629,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<SpringBone3DJointSetting *> &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<SpringBone3DJointSetting *> &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 +1783,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 +1794,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 +1817,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;
}

17
scene/3d/spring_bone_simulator_3d.h
View File

@ -64,8 +64,8 @@ public:
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 @@ public:
protected:
LocalVector<SpringBone3DSetting *> 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 @@ protected:
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 @@ public:
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();
};

154
scene/3d/two_bone_ik_3d.cpp
View File

@ -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,14 +364,17 @@ 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);
}
#ifdef TOOLS_ENABLED
update_gizmos();
_make_gizmo_dirty();
#endif // TOOLS_ENABLED
if (mutable_bone_axes) {
return; // Chain dir will be recaluclated in _update_bone_axis().
}
tb_settings[p_index]->simulation_dirty = true;
}
SkeletonModifier3D::BoneDirection TwoBoneIK3D::get_end_bone_direction(int p_index) const {
@ -381,11 +384,15 @@ 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;
tb_settings[p_index]->simulation_dirty = true;
#ifdef TOOLS_ENABLED
update_gizmos();
_make_gizmo_dirty();
#endif // TOOLS_ENABLED
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;
}
float TwoBoneIK3D::get_end_bone_length(int p_index) const {
@ -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<int> 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();
}

9
scene/3d/two_bone_ik_3d.h
View File

@ -252,12 +252,16 @@ protected:
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 @@ public:
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();
};