Merge pull request #57675 from TokageItLab/fix-blending

6 files changed, 83 insertions, 51 deletions

diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp
index 0a650a8578..11bfcc1a6f 100644
--- a/core/math/quaternion.cpp
+++ b/core/math/quaternion.cpp
@@ -102,6 +102,22 @@ Quaternion Quaternion::inverse() const {
 	return Quaternion(-x, -y, -z, w);
 }
 
+Quaternion Quaternion::log() const {
+	Quaternion src = *this;
+	Vector3 src_v = src.get_axis() * src.get_angle();
+	return Quaternion(src_v.x, src_v.y, src_v.z, 0);
+}
+
+Quaternion Quaternion::exp() const {
+	Quaternion src = *this;
+	Vector3 src_v = Vector3(src.x, src.y, src.z);
+	float theta = src_v.length();
+	if (theta < CMP_EPSILON) {
+		return Quaternion(0, 0, 0, 1);
+	}
+	return Quaternion(src_v.normalized(), theta);
+}
+
 Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) const {
 #ifdef MATH_CHECKS
 	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
@@ -190,6 +206,9 @@ Quaternion::operator String() const {
 }
 
 Vector3 Quaternion::get_axis() const {
+	if (Math::abs(w) > 1 - CMP_EPSILON) {
+		return Vector3(x, y, z);
+	}
 	real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
 	return Vector3(x * r, y * r, z * r);
 }
diff --git a/core/math/quaternion.h b/core/math/quaternion.h
index 38729ac3df..9801746659 100644
--- a/core/math/quaternion.h
+++ b/core/math/quaternion.h
@@ -60,6 +60,8 @@ struct _NO_DISCARD_ Quaternion {
 	Quaternion normalized() const;
 	bool is_normalized() const;
 	Quaternion inverse() const;
+	Quaternion log() const;
+	Quaternion exp() const;
 	_FORCE_INLINE_ real_t dot(const Quaternion &p_q) const;
 	real_t angle_to(const Quaternion &p_to) const;
 
diff --git a/core/variant/variant_call.cpp b/core/variant/variant_call.cpp
index bc29be77fc..17be51eba6 100644
--- a/core/variant/variant_call.cpp
+++ b/core/variant/variant_call.cpp
@@ -1626,6 +1626,8 @@ static void _register_variant_builtin_methods() {
 	bind_method(Quaternion, is_normalized, sarray(), varray());
 	bind_method(Quaternion, is_equal_approx, sarray("to"), varray());
 	bind_method(Quaternion, inverse, sarray(), varray());
+	bind_method(Quaternion, log, sarray(), varray());
+	bind_method(Quaternion, exp, sarray(), varray());
 	bind_method(Quaternion, angle_to, sarray("to"), varray());
 	bind_method(Quaternion, dot, sarray("with"), varray());
 	bind_method(Quaternion, slerp, sarray("to", "weight"), varray());
diff --git a/doc/classes/Quaternion.xml b/doc/classes/Quaternion.xml
index 8a440d9024..48e6317b11 100644
--- a/doc/classes/Quaternion.xml
+++ b/doc/classes/Quaternion.xml
@@ -91,6 +91,11 @@
 				Returns the dot product of two quaternions.
 			</description>
 		</method>
+		<method name="exp" qualifiers="const">
+			<return type="Quaternion" />
+			<description>
+			</description>
+		</method>
 		<method name="get_angle" qualifiers="const">
 			<return type="float" />
 			<description>
@@ -138,6 +143,11 @@
 				Returns the length of the quaternion, squared.
 			</description>
 		</method>
+		<method name="log" qualifiers="const">
+			<return type="Quaternion" />
+			<description>
+			</description>
+		</method>
 		<method name="normalized" qualifiers="const">
 			<return type="Quaternion" />
 			<description>
diff --git a/scene/animation/animation_tree.cpp b/scene/animation/animation_tree.cpp
index e0e94d8632..309c2b5245 100644
--- a/scene/animation/animation_tree.cpp
+++ b/scene/animation/animation_tree.cpp
@@ -273,10 +273,6 @@ double AnimationNode::_blend_node(const StringName &p_subpath, const Vector<Stri
 		}
 	}
 
-	if (!p_seek && p_optimize && !any_valid) { //pointless to go on, all are zero
-		return 0;
-	}
-
 	String new_path;
 	AnimationNode *new_parent;
 
@@ -289,6 +285,10 @@ double AnimationNode::_blend_node(const StringName &p_subpath, const Vector<Stri
 		new_parent = parent;
 		new_path = String(parent->base_path) + String(p_subpath) + "/";
 	}
+
+	if (!p_seek && p_optimize && !any_valid) {
+		return p_node->_pre_process(new_path, new_parent, state, 0, p_seek, p_connections);
+	}
 	return p_node->_pre_process(new_path, new_parent, state, p_time, p_seek, p_connections);
 }
 
@@ -618,6 +618,11 @@ bool AnimationTree::_update_caches(AnimationPlayer *player) {
 							int bone_idx = sk->find_bone(path.get_subname(0));
 							if (bone_idx != -1) {
 								track_xform->bone_idx = bone_idx;
+								Transform3D rest = sk->get_bone_rest(bone_idx);
+								track_xform->init_loc = rest.origin;
+								track_xform->ref_rot = rest.basis.get_rotation_quaternion();
+								track_xform->init_rot = track_xform->ref_rot.log();
+								track_xform->init_scale = rest.basis.get_scale();
 							}
 						}
 
@@ -644,7 +649,6 @@ bool AnimationTree::_update_caches(AnimationPlayer *player) {
 					} break;
 					case Animation::TYPE_BLEND_SHAPE: {
 #ifndef _3D_DISABLED
-
 						if (path.get_subname_count() != 1) {
 							ERR_PRINT("AnimationTree: '" + String(E) + "', blend shape track does not contain a blend shape subname:  '" + String(path) + "'");
 							continue;
@@ -942,23 +946,16 @@ void AnimationTree::_process_graph(double p_delta) {
 
 				real_t blend = (*as.track_blends)[blend_idx] * weight;
 
-				if (blend < CMP_EPSILON) {
-					continue; //nothing to blend
-				}
-
 				switch (ttype) {
 					case Animation::TYPE_POSITION_3D: {
 #ifndef _3D_DISABLED
 						TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
-
 						if (t->process_pass != process_pass) {
 							t->process_pass = process_pass;
-							t->loc = Vector3();
-							t->rot = Quaternion();
-							t->rot_blend_accum = 0;
-							t->scale = Vector3(1, 1, 1);
+							t->loc = t->init_loc;
+							t->rot = t->init_rot;
+							t->scale = t->init_scale;
 						}
-
 						if (track->root_motion) {
 							double prev_time = time - delta;
 							if (!backward) {
@@ -1036,22 +1033,19 @@ void AnimationTree::_process_graph(double p_delta) {
 								continue;
 							}
 
-							t->loc = t->loc.lerp(loc, blend);
+							t->loc += (loc - t->init_loc) * blend;
 						}
 #endif // _3D_DISABLED
 					} break;
 					case Animation::TYPE_ROTATION_3D: {
 #ifndef _3D_DISABLED
 						TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
-
 						if (t->process_pass != process_pass) {
 							t->process_pass = process_pass;
-							t->loc = Vector3();
-							t->rot = Quaternion();
-							t->rot_blend_accum = 0;
-							t->scale = Vector3(1, 1, 1);
+							t->loc = t->init_loc;
+							t->rot = t->init_rot;
+							t->scale = t->init_scale;
 						}
-
 						if (track->root_motion) {
 							double prev_time = time - delta;
 							if (!backward) {
@@ -1097,8 +1091,7 @@ void AnimationTree::_process_graph(double p_delta) {
 										continue;
 									}
 									a->rotation_track_interpolate(i, (double)a->get_length(), &rot[1]);
-									Quaternion q = Quaternion().slerp(rot[0].normalized().inverse() * rot[1].normalized(), blend).normalized();
-									t->rot = (t->rot * q).normalized();
+									t->rot += (rot[1].log() - rot[0].log()) * blend;
 									prev_time = 0;
 								}
 							} else {
@@ -1108,8 +1101,7 @@ void AnimationTree::_process_graph(double p_delta) {
 										continue;
 									}
 									a->rotation_track_interpolate(i, 0, &rot[1]);
-									Quaternion q = Quaternion().slerp(rot[0].normalized().inverse() * rot[1].normalized(), blend).normalized();
-									t->rot = (t->rot * q).normalized();
+									t->rot += (rot[1].log() - rot[0].log()) * blend;
 									prev_time = 0;
 								}
 							}
@@ -1120,8 +1112,7 @@ void AnimationTree::_process_graph(double p_delta) {
 							}
 
 							a->rotation_track_interpolate(i, time, &rot[1]);
-							Quaternion q = Quaternion().slerp(rot[0].normalized().inverse() * rot[1].normalized(), blend).normalized();
-							t->rot = (t->rot * q).normalized();
+							t->rot += (rot[1].log() - rot[0].log()) * blend;
 							prev_time = !backward ? 0 : (double)a->get_length();
 
 						} else {
@@ -1132,29 +1123,22 @@ void AnimationTree::_process_graph(double p_delta) {
 								continue;
 							}
 
-							if (t->rot_blend_accum == 0) {
-								t->rot = rot;
-								t->rot_blend_accum = blend;
-							} else {
-								real_t rot_total = t->rot_blend_accum + blend;
-								t->rot = rot.slerp(t->rot, t->rot_blend_accum / rot_total).normalized();
-								t->rot_blend_accum = rot_total;
+							if (signbit(rot.dot(t->ref_rot))) {
+								rot = -rot;
 							}
+							t->rot += (rot.log() - t->init_rot) * blend;
 						}
 #endif // _3D_DISABLED
 					} break;
 					case Animation::TYPE_SCALE_3D: {
 #ifndef _3D_DISABLED
 						TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
-
 						if (t->process_pass != process_pass) {
 							t->process_pass = process_pass;
-							t->loc = Vector3();
-							t->rot = Quaternion();
-							t->rot_blend_accum = 0;
-							t->scale = Vector3(1, 1, 1);
+							t->loc = t->init_loc;
+							t->rot = t->init_rot;
+							t->scale = t->init_scale;
 						}
-
 						if (track->root_motion) {
 							double prev_time = time - delta;
 							if (!backward) {
@@ -1232,7 +1216,7 @@ void AnimationTree::_process_graph(double p_delta) {
 								continue;
 							}
 
-							t->scale = t->scale.lerp(scale, blend);
+							t->scale += (scale - t->init_scale) * blend;
 						}
 #endif // _3D_DISABLED
 					} break;
@@ -1254,7 +1238,7 @@ void AnimationTree::_process_graph(double p_delta) {
 							continue;
 						}
 
-						t->value = Math::lerp(t->value, value, (float)blend);
+						t->value += value * blend;
 #endif // _3D_DISABLED
 					} break;
 					case Animation::TYPE_VALUE: {
@@ -1271,13 +1255,16 @@ void AnimationTree::_process_graph(double p_delta) {
 							}
 
 							if (t->process_pass != process_pass) {
-								t->value = value;
 								t->process_pass = process_pass;
+								t->value = value;
+								t->value.zero();
 							}
 
-							Variant::interpolate(t->value, value, blend, t->value);
-
+							Variant::blend(t->value, value, blend, t->value);
 						} else {
+							if (blend < CMP_EPSILON) {
+								continue; //nothing to blend
+							}
 							List<int> indices;
 							a->value_track_get_key_indices(i, time, delta, &indices, pingponged);
 
@@ -1289,7 +1276,10 @@ void AnimationTree::_process_graph(double p_delta) {
 
 					} break;
 					case Animation::TYPE_METHOD: {
-						if (delta == 0) {
+						if (blend < CMP_EPSILON) {
+							continue; //nothing to blend
+						}
+						if (!seeked && Math::is_zero_approx(delta)) {
 							continue;
 						}
 						TrackCacheMethod *t = static_cast<TrackCacheMethod *>(track);
@@ -1312,14 +1302,16 @@ void AnimationTree::_process_graph(double p_delta) {
 						real_t bezier = a->bezier_track_interpolate(i, time);
 
 						if (t->process_pass != process_pass) {
-							t->value = bezier;
 							t->process_pass = process_pass;
+							t->value = 0;
 						}
 
-						t->value = Math::lerp(t->value, bezier, blend);
-
+						t->value += bezier * blend;
 					} break;
 					case Animation::TYPE_AUDIO: {
+						if (blend < CMP_EPSILON) {
+							continue; //nothing to blend
+						}
 						TrackCacheAudio *t = static_cast<TrackCacheAudio *>(track);
 
 						if (seeked) {
@@ -1431,6 +1423,9 @@ void AnimationTree::_process_graph(double p_delta) {
 						}
 					} break;
 					case Animation::TYPE_ANIMATION: {
+						if (blend < CMP_EPSILON) {
+							continue; //nothing to blend
+						}
 						TrackCacheAnimation *t = static_cast<TrackCacheAnimation *>(track);
 
 						AnimationPlayer *player2 = Object::cast_to<AnimationPlayer>(t->object);
@@ -1521,6 +1516,7 @@ void AnimationTree::_process_graph(double p_delta) {
 				case Animation::TYPE_POSITION_3D: {
 #ifndef _3D_DISABLED
 					TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
+					t->rot = t->rot.exp();
 
 					if (t->root_motion) {
 						Transform3D xform;
diff --git a/scene/animation/animation_tree.h b/scene/animation/animation_tree.h
index 705ee91c76..3ccb6be073 100644
--- a/scene/animation/animation_tree.h
+++ b/scene/animation/animation_tree.h
@@ -197,9 +197,12 @@ private:
 		bool loc_used = false;
 		bool rot_used = false;
 		bool scale_used = false;
+		Vector3 init_loc = Vector3(0, 0, 0);
+		Quaternion ref_rot = Quaternion(0, 0, 0, 1);
+		Quaternion init_rot = Quaternion(0, 0, 0, 0);
+		Vector3 init_scale = Vector3(1, 1, 1);
 		Vector3 loc;
 		Quaternion rot;
-		real_t rot_blend_accum = 0.0;
 		Vector3 scale;
 
 		TrackCacheTransform() {