Add Math::blend_from_angle() and Quaternion.blend_from() to blend rotations from an initial axis

core/math/math_funcs.h

@@ -433,6 +433,33 @@ class Math {
 		return p_from + Math::angle_difference(p_from, p_to) * p_weight;
 	}
 
+	static _ALWAYS_INLINE_ double blend_from_angle(double p_rest, double p_from, double p_to, double p_weight) {
+		double rot_a = Math::fposmod(p_from, Math_TAU);
+		double rot_b = Math::fposmod(p_to, Math_TAU);
+		double rot_rest = Math::fposmod(p_rest, Math_TAU);
+		if (rot_rest < Math_PI) {
+			rot_a = rot_a > rot_rest + Math_PI ? rot_a - Math_TAU : rot_a;
+			rot_b = rot_b > rot_rest + Math_PI ? rot_b - Math_TAU : rot_b;
+		} else {
+			rot_a = rot_a < rot_rest - Math_PI ? rot_a + Math_TAU : rot_a;
+			rot_b = rot_b < rot_rest - Math_PI ? rot_b + Math_TAU : rot_b;
+		}
+		return Math::fposmod(rot_a + (rot_b - rot_rest) * p_weight, Math_TAU);
+	}
+	static _ALWAYS_INLINE_ float blend_from_angle(float p_rest, float p_from, float p_to, float p_weight) {
+		float rot_a = Math::fposmod(p_from, (float)Math_TAU);
+		float rot_b = Math::fposmod(p_to, (float)Math_TAU);
+		float rot_rest = Math::fposmod(p_rest, (float)Math_TAU);
+		if (rot_rest < (float)Math_PI) {
+			rot_a = rot_a > rot_rest + (float)Math_PI ? rot_a - (float)Math_TAU : rot_a;
+			rot_b = rot_b > rot_rest + (float)Math_PI ? rot_b - (float)Math_TAU : rot_b;
+		} else {
+			rot_a = rot_a < rot_rest - (float)Math_PI ? rot_a + (float)Math_TAU : rot_a;
+			rot_b = rot_b < rot_rest - (float)Math_PI ? rot_b + (float)Math_TAU : rot_b;
+		}
+		return Math::fposmod(rot_a + (rot_b - rot_rest) * p_weight, (float)Math_TAU);
+	}
+
 	static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) {
 		return (p_value - p_from) / (p_to - p_from);
 	}

core/math/quaternion.cpp

@@ -179,6 +179,8 @@ Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const
 #ifdef MATH_CHECKS
 	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
 	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion " + p_b.operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_pre_a.is_normalized(), Quaternion(), "The pre quaternion " + p_pre_a.operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_post_b.is_normalized(), Quaternion(), "The post quaternion " + p_post_b.operator String() + " must be normalized.");
 #endif
 	Quaternion from_q = *this;
 	Quaternion pre_q = p_pre_a;
@@ -230,6 +232,8 @@ Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b
 #ifdef MATH_CHECKS
 	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
 	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion " + p_b.operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_pre_a.is_normalized(), Quaternion(), "The pre quaternion " + p_pre_a.operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_post_b.is_normalized(), Quaternion(), "The post quaternion " + p_post_b.operator String() + " must be normalized.");
 #endif
 	Quaternion from_q = *this;
 	Quaternion pre_q = p_pre_a;
@@ -276,6 +280,15 @@ Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b
 	return q1.slerp(q2, p_weight);
 }
 
+Quaternion Quaternion::blend_from(const Quaternion &p_init, const Quaternion &p_to, real_t p_weight) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion " + p_to.operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_init.is_normalized(), Quaternion(), "The initial quaternion " + p_init.operator String() + " must be normalized.");
+#endif
+	return ((*this) * Quaternion().slerp(p_init.inverse() * p_to, p_weight)).normalized();
+}
+
 Quaternion::operator String() const {
 	return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")";
 }

core/math/quaternion.h

@@ -71,6 +71,7 @@ struct [[nodiscard]] Quaternion {
 	Quaternion slerpni(const Quaternion &p_to, real_t p_weight) const;
 	Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight) const;
 	Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+	Quaternion blend_from(const Quaternion &p_rest, const Quaternion &p_to, real_t p_weight) const;
 
 	Vector3 get_axis() const;
 	real_t get_angle() const;

core/variant/variant_call.cpp

@@ -2091,6 +2091,7 @@ static void _register_variant_builtin_methods_math() {
 	bind_method(Quaternion, slerpni, sarray("to", "weight"), varray());
 	bind_method(Quaternion, spherical_cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray());
 	bind_method(Quaternion, spherical_cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray());
+	bind_method(Quaternion, blend_from, sarray("rest", "to", "weight"), varray());
 	bind_method(Quaternion, get_euler, sarray("order"), varray((int64_t)EulerOrder::YXZ));
 	bind_static_method(Quaternion, from_euler, sarray("euler"), varray());
 	bind_method(Quaternion, get_axis, sarray(), varray());

core/variant/variant_utility.cpp

@@ -545,6 +545,10 @@ double VariantUtilityFunctions::lerp_angle(double from, double to, double weight
 	return Math::lerp_angle(from, to, weight);
 }
 
+double VariantUtilityFunctions::blend_from_angle(double rest, double from, double to, double weight) {
+	return Math::blend_from_angle(rest, from, to, weight);
+}
+
 double VariantUtilityFunctions::inverse_lerp(double from, double to, double weight) {
 	return Math::inverse_lerp(from, to, weight);
 }
@@ -1778,6 +1782,7 @@ void Variant::_register_variant_utility_functions() {
 	FUNCBINDR(lerp_angle, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(inverse_lerp, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(remap, sarray("value", "istart", "istop", "ostart", "ostop"), Variant::UTILITY_FUNC_TYPE_MATH);
+	FUNCBINDR(blend_from_angle, sarray("rest", "from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
 
 	FUNCBINDR(smoothstep, sarray("from", "to", "x"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(move_toward, sarray("from", "to", "delta"), Variant::UTILITY_FUNC_TYPE_MATH);

core/variant/variant_utility.h

@@ -91,6 +91,7 @@ struct VariantUtilityFunctions {
 	static double bezier_derivative(double p_start, double p_control_1, double p_control_2, double p_end, double p_t);
 	static double angle_difference(double from, double to);
 	static double lerp_angle(double from, double to, double weight);
+	static double blend_from_angle(double init, double from, double to, double weight);
 	static double inverse_lerp(double from, double to, double weight);
 	static double remap(double value, double istart, double istop, double ostart, double ostop);
 	static double smoothstep(double from, double to, double val);

doc/classes/@GlobalScope.xml

@@ -176,6 +176,18 @@
 				Returns the point at the given [param t] on a one-dimensional [url=https://en.wikipedia.org/wiki/B%C3%A9zier_curve]Bézier curve[/url] defined by the given [param control_1], [param control_2], and [param end] points.
 			</description>
 		</method>
+		<method name="blend_from_angle">
+			<return type="float" />
+			<param index="0" name="rest" type="float" />
+			<param index="1" name="from" type="float" />
+			<param index="2" name="to" type="float" />
+			<param index="3" name="weight" type="float" />
+			<description>
+				Returns the blended value of the [param from] and [param to] angles with respect to the [param rest], but does not cross over the inverted axis [param rest].
+				This method does not respect the shortest path like [method lerp_angle], but instead does not jump values even if more than one interpolation is stacked unless the same [param rest] is passed to them.
+				This is useful for preventing bone penetration into the character's body when you rotate a [Bone2D].
+			</description>
+		</method>
 		<method name="bytes_to_var">
 			<return type="Variant" />
 			<param index="0" name="bytes" type="PackedByteArray" />

doc/classes/Quaternion.xml

@@ -77,6 +77,17 @@
 				[b]Note:[/b] The magnitude of the floating-point error for this method is abnormally high, so methods such as [code]is_zero_approx[/code] will not work reliably.
 			</description>
 		</method>
+		<method name="blend_from" qualifiers="const">
+			<return type="Quaternion" />
+			<param index="0" name="rest" type="Quaternion" />
+			<param index="1" name="to" type="Quaternion" />
+			<param index="2" name="weight" type="float" />
+			<description>
+				Returns the blended value of the this quaternion and [param to] angles with respect to the [param rest], but does not cross over the inverted axis [param rest].
+				This method does not respect the shortest path like [method slerp], but instead does not jump values even if more than one interpolation is stacked unless the same [param rest] is passed to them.
+				This is useful for preventing bone penetration into the character's body when you rotate a bone of the [Skeleton3D] with passing a quaternion retrieved by [method Skeleton3D.get_bone_rest] to [param rest].
+			</description>
+		</method>
 		<method name="dot" qualifiers="const">
 			<return type="float" />
 			<param index="0" name="with" type="Quaternion" />

scene/3d/look_at_modifier_3d.cpp

@@ -581,7 +581,7 @@ void LookAtModifier3D::_process_modification() {
 			// Interpolate through the rest same as AnimationTree blending for preventing to penetrate the bone into the body.
 			Quaternion rest = skeleton->get_bone_rest(bone).basis.get_rotation_quaternion();
 			float weight = Tween::run_equation(transition_type, ease_type, 1 - remaining, 0.0, 1.0, 1.0);
-			destination = rest * Quaternion().slerp(rest.inverse() * from_q, 1 - weight) * Quaternion().slerp(rest.inverse() * destination, weight);
+			destination = rest.blend_from(rest, from_q, 1 - weight).blend_from(rest, destination, weight);
 		} else {
 			destination = from_q.slerp(destination, Tween::run_equation(transition_type, ease_type, 1 - remaining, 0.0, 1.0, 1.0));
 		}

scene/animation/animation_mixer.cpp

@@ -1408,7 +1408,7 @@ void AnimationMixer::_blend_process(double p_delta, bool p_update_only) {
 								rot[0] = post_process_key_value(a, i, rot[0], t->object_id, t->bone_idx);
 								a->try_rotation_track_interpolate(i, end, &rot[1]);
 								rot[1] = post_process_key_value(a, i, rot[1], t->object_id, t->bone_idx);
-								root_motion_cache.rot = (root_motion_cache.rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
+								root_motion_cache.rot = root_motion_cache.rot.blend_from(rot[0], rot[1], blend);
 								prev_time = start;
 							}
 						} else {
@@ -1420,7 +1420,7 @@ void AnimationMixer::_blend_process(double p_delta, bool p_update_only) {
 								rot[0] = post_process_key_value(a, i, rot[0], t->object_id, t->bone_idx);
 								a->try_rotation_track_interpolate(i, start, &rot[1]);
 								rot[1] = post_process_key_value(a, i, rot[1], t->object_id, t->bone_idx);
-								root_motion_cache.rot = (root_motion_cache.rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
+								root_motion_cache.rot = root_motion_cache.rot.blend_from(rot[0], rot[1], blend);
 								prev_time = end;
 							}
 						}
@@ -1431,7 +1431,7 @@ void AnimationMixer::_blend_process(double p_delta, bool p_update_only) {
 						rot[0] = post_process_key_value(a, i, rot[0], t->object_id, t->bone_idx);
 						a->try_rotation_track_interpolate(i, time, &rot[1]);
 						rot[1] = post_process_key_value(a, i, rot[1], t->object_id, t->bone_idx);
-						root_motion_cache.rot = (root_motion_cache.rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
+						root_motion_cache.rot = root_motion_cache.rot.blend_from(rot[0], rot[1], blend);
 						prev_time = !backward ? start : end;
 					}
 					{
@@ -1441,7 +1441,7 @@ void AnimationMixer::_blend_process(double p_delta, bool p_update_only) {
 							continue;
 						}
 						rot = post_process_key_value(a, i, rot, t->object_id, t->bone_idx);
-						t->rot = (t->rot * Quaternion().slerp(t->init_rot.inverse() * rot, blend)).normalized();
+						t->rot = t->rot.blend_from(t->init_rot, rot, blend);
 					}
 #endif // _3D_DISABLED
 				} break;
@@ -1587,21 +1587,8 @@ void AnimationMixer::_blend_process(double p_delta, bool p_update_only) {
 						// Special case for angle interpolation.
 						if (t->is_using_angle) {
 							// For blending consistency, it prevents rotation of more than 180 degrees from init_value.
-							// This is the same as for Quaternion blends.
-							float rot_a = t->value;
-							float rot_b = value;
-							float rot_init = t->init_value;
-							rot_a = Math::fposmod(rot_a, (float)Math_TAU);
-							rot_b = Math::fposmod(rot_b, (float)Math_TAU);
-							rot_init = Math::fposmod(rot_init, (float)Math_TAU);
-							if (rot_init < Math_PI) {
-								rot_a = rot_a > rot_init + Math_PI ? rot_a - Math_TAU : rot_a;
-								rot_b = rot_b > rot_init + Math_PI ? rot_b - Math_TAU : rot_b;
-							} else {
-								rot_a = rot_a < rot_init - Math_PI ? rot_a + Math_TAU : rot_a;
-								rot_b = rot_b < rot_init - Math_PI ? rot_b + Math_TAU : rot_b;
-							}
-							t->value = Math::fposmod(rot_a + (rot_b - rot_init) * (float)blend, (float)Math_TAU);
+							// This is the same as for Quaternion's blend_from().
+							t->value = Math::blend_from_angle(t->init_value, t->value, value, blend);
 						} else {
 							value = Animation::cast_to_blendwise(value);
 							if (t->init_value.is_array()) {