Add mutable_bone_axes to IKs

Author: TokageItLab

doc/classes/IKModifier3D.xml

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

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.
 		</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>

editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp

@@ -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);
 							}
 						}
 					}

editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp

@@ -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));

editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp

@@ -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);
 	}
 

scene/3d/bone_constraint_3d.cpp

@@ -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);

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,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,14 +479,67 @@ void ChainIK3D::_update_joints(int p_index) {
 	}
 
 #ifdef TOOLS_ENABLED
-	update_gizmos();
+	_make_gizmo_dirty();
 #endif // TOOLS_ENABLED
 }
 
 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();
 }