Add slerp_with_rest static func to Animation class

Author: TokageItLab

scene/3d/look_at_modifier_3d.cpp

@@ -30,6 +30,8 @@
 
 #include "look_at_modifier_3d.h"
 
+#include "scene/resources/animation.h"
+
 void LookAtModifier3D::_validate_property(PropertyInfo &p_property) const {
 	if (Engine::get_singleton()->is_editor_hint() && (p_property.name == "bone_name" || p_property.name == "origin_bone_name")) {
 		Skeleton3D *skeleton = get_skeleton();
@@ -587,7 +589,7 @@ void LookAtModifier3D::_process_modification(double p_delta) {
 			// 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 = Animation::slerp_with_rest(Animation::slerp_with_rest(rest, from_q, 1 - weight, rest), destination, weight, rest);
 		} 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

@@ -1406,7 +1406,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 = Animation::slerp_with_rest(root_motion_cache.rot, rot[1], blend, rot[0]);
 								prev_time = start;
 							}
 						} else {
@@ -1418,7 +1418,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 = Animation::slerp_with_rest(root_motion_cache.rot, rot[1], blend, rot[0]);
 								prev_time = end;
 							}
 						}
@@ -1429,7 +1429,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 = Animation::slerp_with_rest(root_motion_cache.rot, rot[1], blend, rot[0]);
 						prev_time = !backward ? start : end;
 					}
 					{
@@ -1439,7 +1439,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 = Animation::slerp_with_rest(t->rot, rot, blend, t->init_rot);
 					}
 #endif // _3D_DISABLED
 				} break;
@@ -1585,21 +1585,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 with Quaternion blending.
+							t->value = Animation::slerp_with_rest((double)t->value, (double)value, blend, (double)t->init_value);
 						} else {
 							value = Animation::cast_to_blendwise(value);
 							if (t->init_value.is_array()) {

scene/resources/animation.cpp

@@ -5671,6 +5671,30 @@ bool Animation::_fetch_compressed_by_index(uint32_t p_compressed_track, int p_in
 	return false;
 }
 
+// Helper functions for Rotation.
+double Animation::slerp_with_rest(double p_from, double p_to, double p_weight, double p_rest) {
+	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);
+}
+
+Quaternion Animation::slerp_with_rest(const Quaternion &p_from, const Quaternion &p_to, real_t p_weight, const Quaternion &p_rest) {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!p_from.is_normalized(), Quaternion(), "The start quaternion must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_rest.is_normalized(), Quaternion(), "The rest quaternion must be normalized.");
+#endif
+	return (p_from * Quaternion().slerp(p_rest.inverse() * p_to, p_weight)).normalized();
+}
+
 // Helper math functions for Variant.
 bool Animation::is_variant_interpolatable(const Variant p_value) {
 	Variant::Type type = p_value.get_type();

scene/resources/animation.h

@@ -540,6 +540,10 @@ class Animation : public Resource {
 	void optimize(real_t p_allowed_velocity_err = 0.01, real_t p_allowed_angular_err = 0.01, int p_precision = 3);
 	void compress(uint32_t p_page_size = 8192, uint32_t p_fps = 120, float p_split_tolerance = 4.0); // 4.0 seems to be the split tolerance sweet spot from many tests.
 
+	// Helper functions for Rotation.
+	static double slerp_with_rest(double p_from, double p_to, double p_weight, double p_rest = 0.0); // Deterministic slerp to prevent to cross the inverted rest axis.
+	static Quaternion slerp_with_rest(const Quaternion &p_from, const Quaternion &p_to, real_t p_weight, const Quaternion &p_rest = Quaternion()); // Deterministic slerp to prevent to cross the inverted rest axis.
+
 	// Helper functions for Variant.
 	static bool is_variant_interpolatable(const Variant p_value);
 	static bool validate_type_match(const Variant &p_from, Variant &r_to);