Add new methods for setting and getting bone resistance in IK classes

doc_classes/ManyBoneIK3D.xml

@@ -85,6 +85,13 @@
 				Returns the radius of the limit cone for the kusudama at the specified index.
 			</description>
 		</method>
+		<method name="get_kusudama_resistance" qualifiers="const">
+			<return type="float" />
+			<param index="0" name="index" type="int" />
+			<description>
+				Returns the resistance of a bone. resistance is how much the bone prefers to remain in its comfortable region.
+			</description>
+		</method>
 		<method name="get_kusudama_twist" qualifiers="const">
 			<return type="Vector2" />
 			<param index="0" name="index" type="int" />
@@ -234,6 +241,14 @@
 				Sets the radius of the limit cone for the kusudama at the specified index.
 			</description>
 		</method>
+		<method name="set_kusudama_resistance">
+			<return type="void" />
+			<param index="0" name="index" type="int" />
+			<param index="1" name="resistance" type="float" />
+			<description>
+				Sets the resistance of a bone. resistance is how much the bone prefers to remain in its comfortable region.
+			</description>
+		</method>
 		<method name="set_kusudama_twist">
 			<return type="void" />
 			<param index="0" name="index" type="int" />

src/ik_bone_3d.cpp

@@ -298,6 +298,13 @@ bool IKBone3D::is_axially_constrained() {
 	return get_constraint()->is_axially_constrained();
 }
 
+void IKBone3D::pull_back_toward_allowable_region() {
+	Ref<IKKusudama3D> current_constraint = get_constraint();
+	if (current_constraint.is_valid()) {
+		current_constraint->set_axes_to_returnfulled(get_bone_direction_transform(), get_ik_transform(), get_constraint_orientation_transform(), cos_half_return_damp, return_damp);
+	}
+}
+
 Vector<float> &IKBone3D::get_cos_half_returnfullness_dampened() {
 	return cos_half_returnfulness_dampened;
 }

src/ik_bone_3d.h

@@ -81,6 +81,7 @@ class IKBone3D : public Resource {
 	void set_half_returnfullness_dampened(const Vector<float> &p_value);
 	void set_stiffness(double p_stiffness);
 	double get_stiffness() const;
+	void pull_back_toward_allowable_region();
 	bool is_axially_constrained();
 	bool is_orientationally_constrained();
 	Transform3D get_bone_direction_global_pose() const;

src/ik_bone_segment_3d.cpp

@@ -182,6 +182,20 @@ void IKBoneSegment3D::_set_optimal_rotation(Ref<IKBone3D> p_for_bone, PackedVect
 			_update_tip_headings(p_for_bone, &tip_headings_uniform);
 			double current_msd = _get_manual_msd(tip_headings_uniform, target_headings, heading_weights);
 			if (current_msd <= previous_deviation * 1.0001) {
+				if (p_for_bone->get_constraint().is_valid() && !Math::is_zero_approx(p_for_bone->get_constraint()->get_resistance())) {
+					if (bone_damp != -1) {
+						float returnfulness = p_for_bone->get_constraint()->get_resistance();
+						float dampened_angle = p_for_bone->get_stiffness() * bone_damp * returnfulness;
+						float total_iterations_square = total_iterations * total_iterations;
+						float scaled_dampened_angle = dampened_angle * ((total_iterations_square - (current_iteration * current_iteration)) / total_iterations_square);
+						float cos_half_angle = Math::cos(0.5f * scaled_dampened_angle);
+						p_for_bone->get_constraint()->set_axes_to_returnfulled(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_orientation_transform(), cos_half_angle, scaled_dampened_angle);
+					} else {
+						p_for_bone->get_constraint()->set_axes_to_returnfulled(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_orientation_transform(), p_for_bone->get_cos_half_returnfullness_dampened()[current_iteration], p_for_bone->get_cos_half_returnfullness_dampened()[current_iteration]);
+					}
+					_update_tip_headings(p_for_bone, &tip_headings);
+					current_msd = _get_manual_msd(tip_headings, target_headings, heading_weights);
+				}
 				previous_deviation = current_msd;
 				got_closer = true;
 				break;
@@ -435,3 +449,13 @@ void IKBoneSegment3D::_finalize_segment(Ref<IKBone3D> p_current_tip) {
 	bones.clear();
 	create_bone_list(bones, false);
 }
+
+void IKBoneSegment3D::update_returnfulness_damp(int32_t p_iterations) {
+	for (Ref<IKBone3D> bone : bones) {
+		if (bone.is_null()) {
+			continue;
+		}
+		bone->pull_back_toward_allowable_region();
+		previous_deviation = INFINITY;
+	}
+}

src/ik_bone_segment_3d.h

@@ -95,6 +95,7 @@ class IKBoneSegment3D : public Resource {
 	Vector<Ref<IKBoneSegment3D>> get_child_segments() const;
 	void create_bone_list(Vector<Ref<IKBone3D>> &p_list, bool p_recursive = false, bool p_debug_skeleton = false) const;
 	Ref<IKBone3D> get_ik_bone(BoneId p_bone) const;
+	void update_returnfulness_damp(int32_t p_iterations);
 	void generate_default_segments(Vector<Ref<IKEffectorTemplate3D>> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik);
 	IKBoneSegment3D() {}
 	IKBoneSegment3D(Skeleton3D *p_skeleton, StringName p_root_bone_name, Vector<Ref<IKEffectorTemplate3D>> &p_pins, ManyBoneIK3D *p_many_bone_ik, const Ref<IKBoneSegment3D> &p_parent = nullptr,

src/ik_kusudama_3d.cpp

@@ -122,11 +122,11 @@ void IKKusudama3D::set_snap_to_twist_limit(Ref<IKNode3D> p_bone_direction, Ref<I
 	Transform3D global_transform_to_set = p_to_set->get_global_transform();
 	Basis parent_global_inverse = p_to_set->get_parent()->get_global_transform().basis.inverse();
 	Basis global_twist_center = global_transform_constraint.basis * twist_center_rot;
-	Basis align_rot = (global_twist_center.inverse() * global_transform_to_set.basis).orthonormalized();
+	Basis align_rot = (global_twist_center.inverse() * global_transform_to_set.basis).orthogonalized();
 	Quaternion twist_rotation, swing_rotation; // Hold the ik transform's decomposed swing and twist away from global_twist_centers's global basis.
 	get_swing_twist(align_rot.get_rotation_quaternion(), Vector3(0, 1, 0), swing_rotation, twist_rotation);
 	twist_rotation = IKBoneSegment3D::clamp_to_quadrance_angle(twist_rotation, twist_half_range_half_cos);
-	Basis recomposition = (global_twist_center * (swing_rotation * twist_rotation)).orthonormalized();
+	Basis recomposition = (global_twist_center * (swing_rotation * twist_rotation)).orthogonalized();
 	Basis rotation = parent_global_inverse * recomposition;
 	p_to_set->set_transform(Transform3D(rotation, p_to_set->get_transform().origin));
 }
@@ -377,6 +377,50 @@ void IKKusudama3D::snap_to_orientation_limit(Ref<IKNode3D> bone_direction, Ref<I
 	}
 }
 
+void IKKusudama3D::set_axes_to_returnfulled(Ref<IKNode3D> bone_direction, Ref<IKNode3D> to_set, Ref<IKNode3D> limiting_axes, float cos_half_returnfullness, float angle_returnfullness) {
+	if (bone_direction.is_null() || to_set.is_null() || limiting_axes.is_null() || resistance <= 0.0) {
+		return;
+	}
+	Quaternion rotation = bone_direction->get_global_transform().basis.get_rotation_quaternion();
+	Quaternion twist_rotation, swing_rotation;
+	get_swing_twist(rotation, Vector3(0, 1, 0), swing_rotation, twist_rotation);
+	if (orientationally_constrained) {
+		Vector3 origin = bone_direction->get_global_transform().origin;
+		Vector3 limiting_origin = limiting_axes->get_global_transform().origin;
+		Vector3 bone_dir_xform = bone_direction->get_global_transform().xform(Vector3(0.0, 1.0, 0.0));
+
+		bone_ray->set_point_1(limiting_origin);
+		bone_ray->set_point_2(bone_dir_xform);
+
+		Vector3 in_point = bone_ray->get_point_2();
+		Vector3 path_point = local_point_on_path_sequence(in_point, limiting_axes);
+		in_point -= origin;
+		path_point -= origin;
+
+		Quaternion to_clamp = Quaternion(in_point.normalized(), path_point.normalized());
+		to_clamp = clamp_to_quadrance_angle(to_clamp, cos_half_returnfullness).normalized();
+		to_set->rotate_local_with_global(to_clamp);
+	}
+	if (axially_constrained) {
+		double angle_to_twist_mid = angle_to_twist_center(bone_direction, limiting_axes);
+		double clamped_angle = CLAMP(angle_to_twist_mid, -angle_returnfullness, angle_returnfullness);
+		Vector3 bone_axis_y = bone_direction->get_global_transform().xform(Vector3(0, 1, 0)).normalized();
+		rotation = Quaternion(bone_axis_y, clamped_angle);
+		to_set->rotate_local_with_global(rotation, false);
+	}
+}
+
+double IKKusudama3D::angle_to_twist_center(Ref<IKNode3D> bone_direction, Ref<IKNode3D> limiting_axes) {
+	if (!is_axially_constrained()) {
+		return 0;
+	}
+	Quaternion inverse_rotation = limiting_axes->get_global_transform().basis.get_rotation_quaternion().inverse();
+	Quaternion align_rotation = inverse_rotation * bone_direction->get_global_transform().basis.get_rotation_quaternion();
+	Vector3 twisted_direction = align_rotation.xform(Vector3(0, 0, 1));
+	Quaternion to_mid = Quaternion(twisted_direction, twist_center_vec);
+	return to_mid.get_angle() * to_mid.get_axis().y;
+}
+
 bool IKKusudama3D::is_nan_vector(const Vector3 &vec) {
 	return Math::is_nan(vec.x) || Math::is_nan(vec.y) || Math::is_nan(vec.z);
 }

src/ik_kusudama_3d.h

@@ -116,6 +116,7 @@ class IKKusudama3D : public Resource {
 	static Quaternion get_quaternion_axis_angle(const Vector3 &p_axis, real_t p_angle);
 
 public:
+	double angle_to_twist_center(Ref<IKNode3D> p_bone_direction, Ref<IKNode3D> p_limiting_axes);
 	/**
 	 * Presumes the input axes are the bone's localAxes, and rotates
 	 * them to satisfy the snap limits.
@@ -198,6 +199,7 @@ class IKKusudama3D : public Resource {
 	float get_resistance();
 	void set_resistance(float p_resistance);
 	static Quaternion clamp_to_quadrance_angle(Quaternion p_rotation, double p_cos_half_angle);
+	void set_axes_to_returnfulled(Ref<IKNode3D> bone_direction, Ref<IKNode3D> to_set, Ref<IKNode3D> limiting_axes, float cos_half_returnfullness, float angle_returnfullness);
 };
 
 #endif // IK_KUSUDAMA_3D_H

src/many_bone_ik_3d.cpp

@@ -194,6 +194,8 @@ void ManyBoneIK3D::_get_property_list(List<PropertyInfo> *p_list) const {
 			bone_name.hint_string = "";
 		}
 		p_list->push_back(bone_name);
+		p_list->push_back(
+				PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/resistance", PROPERTY_HINT_RANGE, "0,1,0.01,exp", constraint_usage));
 		p_list->push_back(
 				PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/twist_from", PROPERTY_HINT_RANGE, "-359.9,359.9,0.1,radians,exp", constraint_usage));
 		p_list->push_back(
@@ -259,6 +261,9 @@ bool ManyBoneIK3D::_get(const StringName &p_name, Variant &r_ret) const {
 			ERR_FAIL_INDEX_V(index, constraint_names.size(), false);
 			r_ret = constraint_names[index];
 			return true;
+		} else if (what == "resistance") {
+			r_ret = get_kusudama_resistance(index);
+			return true;
 		} else if (what == "twist_from") {
 			r_ret = get_kusudama_twist(index).x;
 			return true;
@@ -336,6 +341,8 @@ bool ManyBoneIK3D::_set(const StringName &p_name, const Variant &p_value) {
 		}
 		if (what == "bone_name") {
 			_set_constraint_name(index, p_value);
+		} else if (what == "resistance") {
+			set_kusudama_resistance(index, p_value);
 			return true;
 		} else if (what == "twist_from") {
 			Vector2 twist_from = get_kusudama_twist(index);
@@ -376,6 +383,8 @@ bool ManyBoneIK3D::_set(const StringName &p_name, const Variant &p_value) {
 void ManyBoneIK3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_constraint_name", "index", "name"), &ManyBoneIK3D::_set_constraint_name);
 	ClassDB::bind_method(D_METHOD("set_pin_count", "count"), &ManyBoneIK3D::_set_pin_count);
+	ClassDB::bind_method(D_METHOD("set_kusudama_resistance", "index", "resistance"), &ManyBoneIK3D::set_kusudama_resistance);
+	ClassDB::bind_method(D_METHOD("get_kusudama_resistance", "index"), &ManyBoneIK3D::get_kusudama_resistance);
 	ClassDB::bind_method(D_METHOD("get_constraint_twist_transform", "index"), &ManyBoneIK3D::get_constraint_twist_transform);
 	ClassDB::bind_method(D_METHOD("set_constraint_twist_transform", "index", "transform"), &ManyBoneIK3D::set_constraint_twist_transform);
 	ClassDB::bind_method(D_METHOD("get_constraint_orientation_transform", "index"), &ManyBoneIK3D::get_constraint_orientation_transform);
@@ -462,12 +471,14 @@ void ManyBoneIK3D::_set_constraint_count(int32_t p_count) {
 	kusudama_twist.resize(p_count);
 	kusudama_limit_cone_count.resize(p_count);
 	kusudama_limit_cones.resize(p_count);
+	bone_resistance.resize(p_count);
 	for (int32_t constraint_i = p_count; constraint_i-- > old_count;) {
 		constraint_names.write[constraint_i] = String();
 		kusudama_limit_cone_count.write[constraint_i] = 0;
 		kusudama_limit_cones.write[constraint_i].resize(1);
 		kusudama_limit_cones.write[constraint_i].write[0] = Vector4(0, 1, 0, 0.01745f);
 		kusudama_twist.write[constraint_i] = Vector2(0, 0.01745f);
+		bone_resistance.write[constraint_i] = 0.0f;
 	}
 	set_dirty();
 	notify_property_list_changed();
@@ -708,6 +719,12 @@ void ManyBoneIK3D::_process_modification() {
 		return;
 	}
 	_update_ik_bones_transform();
+	for (Ref<IKBoneSegment3D> segmented_skeleton : segmented_skeletons) {
+		if (segmented_skeleton.is_null()) {
+			continue;
+		}
+		segmented_skeleton->update_returnfulness_damp(get_iterations_per_frame());
+	}
 	for (int32_t i = 0; i < get_iterations_per_frame(); i++) {
 		real_t adjusted_damp = get_default_damp() / get_iterations_per_frame();
 		for (Ref<IKBoneSegment3D> segmented_skeleton : segmented_skeletons) {
@@ -776,6 +793,7 @@ void ManyBoneIK3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new_) {
 			const Vector2 axial_limit = get_kusudama_twist(constraint_i);
 			constraint->enable_axial_limits();
 			constraint->set_axial_limits(axial_limit.x, axial_limit.y);
+			constraint->set_resistance(get_kusudama_resistance(constraint_i));
 			ik_bone_3d->add_constraint(constraint);
 			constraint->_update_constraint(ik_bone_3d->get_constraint_twist_transform());
 			break;
@@ -840,6 +858,7 @@ void ManyBoneIK3D::remove_constraint(int32_t p_index) {
 	kusudama_limit_cone_count.remove_at(p_index);
 	kusudama_limit_cones.remove_at(p_index);
 	kusudama_twist.remove_at(p_index);
+	bone_resistance.remove_at(p_index);
 
 	constraint_count--;
 
@@ -1062,6 +1081,33 @@ Ref<IKNode3D> ManyBoneIK3D::get_godot_skeleton_transform() {
 	return godot_skeleton_transform;
 }
 
+void ManyBoneIK3D::set_kusudama_resistance(int32_t p_index, real_t p_resistance) {
+	ERR_FAIL_INDEX(p_index, constraint_names.size());
+	String bone_name = constraint_names[p_index];
+	bone_resistance.write[p_index] = p_resistance;
+	for (Ref<IKBoneSegment3D> segmented_skeleton : segmented_skeletons) {
+		if (segmented_skeleton.is_null()) {
+			continue;
+		}
+		Ref<IKBone3D> ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name));
+		if (ik_bone.is_null()) {
+			continue;
+		}
+		if (ik_bone->get_constraint().is_null()) {
+			continue;
+		}
+		ik_bone->get_constraint()->set_resistance(p_resistance);
+		ik_bone->set_skeleton_bone_pose(get_skeleton());
+		break;
+	}
+	set_dirty();
+}
+
+real_t ManyBoneIK3D::get_kusudama_resistance(int32_t p_index) const {
+	ERR_FAIL_INDEX_V(p_index, constraint_names.size(), 0.0f);
+	return bone_resistance[p_index];
+}
+
 void ManyBoneIK3D::add_constraint() {
 	int32_t old_count = constraint_count;
 	_set_constraint_count(constraint_count + 1);
@@ -1070,6 +1116,7 @@ void ManyBoneIK3D::add_constraint() {
 	kusudama_limit_cones.write[old_count].resize(1);
 	kusudama_limit_cones.write[old_count].write[0] = Vector4(0, 1, 0, Math_PI);
 	kusudama_twist.write[old_count] = Vector2(0, Math_PI);
+	bone_resistance.write[old_count] = 0.0f;
 	set_dirty();
 }
 

src/many_bone_ik_3d.h

@@ -55,6 +55,7 @@ class ManyBoneIK3D : public SkeletonModifier3D {
 	Vector<Ref<IKBone3D>> bone_list;
 	Vector<Vector2> kusudama_twist;
 	Vector<float> bone_damp;
+	Vector<float> bone_resistance;
 	Vector<Vector<Vector4>> kusudama_limit_cones;
 	Vector<int> kusudama_limit_cone_count;
 	float MAX_KUSUDAMA_LIMIT_CONES = 10;
@@ -132,6 +133,8 @@ class ManyBoneIK3D : public SkeletonModifier3D {
 	int32_t find_pin(String p_string) const;
 	int32_t get_constraint_count() const;
 	StringName get_constraint_name(int32_t p_index) const;
+	void set_kusudama_resistance(int32_t p_index, real_t p_resistance);
+	real_t get_kusudama_resistance(int32_t p_index) const;
 	void set_constraint_twist_transform(int32_t p_index, Transform3D p_transform);
 	Transform3D get_constraint_twist_transform(int32_t p_index) const;
 	void set_constraint_orientation_transform(int32_t p_index, Transform3D p_transform);