Add root motion accumulator to fix broken RootMotionView

4 files changed, 149 insertions, 29 deletions

diff --git a/doc/classes/AnimationTree.xml b/doc/classes/AnimationTree.xml
index 98256f0a38..ed98f47f58 100644
--- a/doc/classes/AnimationTree.xml
+++ b/doc/classes/AnimationTree.xml
@@ -33,7 +33,7 @@
 		<method name="get_root_motion_position" qualifiers="const">
 			<return type="Vector3" />
 			<description>
-				Retrieve the motion of position with the [member root_motion_track] as a [Vector3] that can be used elsewhere.
+				Retrieve the motion delta of position with the [member root_motion_track] as a [Vector3] that can be used elsewhere.
 				If [member root_motion_track] is not a path to a track of type [constant Animation.TYPE_POSITION_3D], returns [code]Vector3(0, 0, 0)[/code].
 				See also [member root_motion_track] and [RootMotionView].
 				The most basic example is applying position to [CharacterBody3D]:
@@ -50,12 +50,46 @@
 				    move_and_slide()
 				[/gdscript]
 				[/codeblocks]
+				By using this in combination with [method get_root_motion_position_accumulator], you can apply the root motion position more correctly to account for the rotation of the node.
+				[codeblocks]
+				[gdscript]
+				func _process(delta):
+				    if Input.is_action_just_pressed("animate"):
+				        state_machine.travel("Animate")
+				    set_quaternion(get_quaternion() * animation_tree.get_root_motion_rotation())
+				    var velocity: Vector3 = (animation_tree.get_root_motion_rotation_accumulator().inverse() * get_quaternion()) * animation_tree.get_root_motion_position() / delta
+				    set_velocity(velocity)
+				    move_and_slide()
+				[/gdscript]
+				[/codeblocks]
+			</description>
+		</method>
+		<method name="get_root_motion_position_accumulator" qualifiers="const">
+			<return type="Vector3" />
+			<description>
+				Retrieve the blended value of the position tracks with the [member root_motion_track] as a [Vector3] that can be used elsewhere.
+				This is useful in cases where you want to respect the initial key values of the animation.
+				For example, if an animation with only one key [code]Vector3(0, 0, 0)[/code] is played in the previous frame and then an animation with only one key [code]Vector3(1, 0, 1)[/code] is played in the next frame, the difference can be calculated as follows:
+				[codeblocks]
+				[gdscript]
+				var prev_root_motion_position_accumulator: Vector3
+
+				func _process(delta):
+				    if Input.is_action_just_pressed("animate"):
+				        state_machine.travel("Animate")
+				    var current_root_motion_position_accumulator: Vector3 = animation_tree.get_root_motion_position_accumulator()
+				    var difference: Vector3 = current_root_motion_position_accumulator - prev_root_motion_position_accumulator
+				    prev_root_motion_position_accumulator = current_root_motion_position_accumulator
+				    transform.origin += difference
+				[/gdscript]
+				[/codeblocks]
+				However, if the animation loops, an unintended discrete change may occur, so this is only useful for some simple use cases.
 			</description>
 		</method>
 		<method name="get_root_motion_rotation" qualifiers="const">
 			<return type="Quaternion" />
 			<description>
-				Retrieve the motion of rotation with the [member root_motion_track] as a [Quaternion] that can be used elsewhere.
+				Retrieve the motion delta of rotation with the [member root_motion_track] as a [Quaternion] that can be used elsewhere.
 				If [member root_motion_track] is not a path to a track of type [constant Animation.TYPE_ROTATION_3D], returns [code]Quaternion(0, 0, 0, 1)[/code].
 				See also [member root_motion_track] and [RootMotionView].
 				The most basic example is applying rotation to [CharacterBody3D]:
@@ -69,10 +103,33 @@
 				[/codeblocks]
 			</description>
 		</method>
+		<method name="get_root_motion_rotation_accumulator" qualifiers="const">
+			<return type="Quaternion" />
+			<description>
+				Retrieve the blended value of the rotation tracks with the [member root_motion_track] as a [Quaternion] that can be used elsewhere.
+				This is necessary to apply the root motion position correctly, taking rotation into account. See also [method get_root_motion_position].
+				Also, this is useful in cases where you want to respect the initial key values of the animation.
+				For example, if an animation with only one key [code]Quaternion(0, 0, 0, 1)[/code] is played in the previous frame and then an animation with only one key [code]Quaternion(0, 0.707, 0, 0.707)[/code] is played in the next frame, the difference can be calculated as follows:
+				[codeblocks]
+				[gdscript]
+				var prev_root_motion_rotation_accumulator: Quaternion
+
+				func _process(delta):
+				    if Input.is_action_just_pressed("animate"):
+				        state_machine.travel("Animate")
+				    var current_root_motion_rotation_accumulator: Quaternion = animation_tree.get_root_motion_Quaternion_accumulator()
+				    var difference: Quaternion = prev_root_motion_rotation_accumulator.inverse() * current_root_motion_rotation_accumulator
+				    prev_root_motion_rotation_accumulator = current_root_motion_rotation_accumulator
+				    transform.basis *= difference
+				[/gdscript]
+				[/codeblocks]
+				However, if the animation loops, an unintended discrete change may occur, so this is only useful for some simple use cases.
+			</description>
+		</method>
 		<method name="get_root_motion_scale" qualifiers="const">
 			<return type="Vector3" />
 			<description>
-				Retrieve the motion of scale with the [member root_motion_track] as a [Vector3] that can be used elsewhere.
+				Retrieve the motion delta of scale with the [member root_motion_track] as a [Vector3] that can be used elsewhere.
 				If [member root_motion_track] is not a path to a track of type [constant Animation.TYPE_SCALE_3D], returns [code]Vector3(0, 0, 0)[/code].
 				See also [member root_motion_track] and [RootMotionView].
 				The most basic example is applying scale to [CharacterBody3D]:
@@ -92,6 +149,27 @@
 				[/codeblocks]
 			</description>
 		</method>
+		<method name="get_root_motion_scale_accumulator" qualifiers="const">
+			<return type="Vector3" />
+			<description>
+				Retrieve the blended value of the scale tracks with the [member root_motion_track] as a [Vector3] that can be used elsewhere.
+				For example, if an animation with only one key [code]Vector3(1, 1, 1)[/code] is played in the previous frame and then an animation with only one key [code]Vector3(2, 2, 2)[/code] is played in the next frame, the difference can be calculated as follows:
+				[codeblocks]
+				[gdscript]
+				var prev_root_motion_scale_accumulator: Vector3
+
+				func _process(delta):
+				    if Input.is_action_just_pressed("animate"):
+				        state_machine.travel("Animate")
+				    var current_root_motion_scale_accumulator: Vector3 = animation_tree.get_root_motion_scale_accumulator()
+				    var difference: Vector3 = current_root_motion_scale_accumulator - prev_root_motion_scale_accumulator
+				    prev_root_motion_scale_accumulator = current_root_motion_scale_accumulator
+				    transform.basis = transform.basis.scaled(difference)
+				[/gdscript]
+				[/codeblocks]
+				However, if the animation loops, an unintended discrete change may occur, so this is only useful for some simple use cases.
+			</description>
+		</method>
 	</methods>
 	<members>
 		<member name="active" type="bool" setter="set_active" getter="is_active" default="false">
diff --git a/scene/animation/animation_tree.cpp b/scene/animation/animation_tree.cpp
index 7c2edef1de..9f9916c1c6 100644
--- a/scene/animation/animation_tree.cpp
+++ b/scene/animation/animation_tree.cpp
@@ -567,6 +567,10 @@ bool AnimationTree::_update_caches(AnimationPlayer *player) {
 	List<StringName> sname;
 	player->get_animation_list(&sname);
 
+	root_motion_cache.loc = Vector3(0, 0, 0);
+	root_motion_cache.rot = Quaternion(0, 0, 0, 1);
+	root_motion_cache.scale = Vector3(1, 1, 1);
+
 	Ref<Animation> reset_anim;
 	bool has_reset_anim = player->has_animation(SceneStringNames::get_singleton()->RESET);
 	if (has_reset_anim) {
@@ -1035,14 +1039,13 @@ void AnimationTree::_process_graph(double p_delta) {
 				case Animation::TYPE_POSITION_3D: {
 					TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
 					if (track->root_motion) {
-						t->loc = Vector3(0, 0, 0);
-						t->rot = Quaternion(0, 0, 0, 1);
-						t->scale = Vector3(1, 1, 1);
-					} else {
-						t->loc = t->init_loc;
-						t->rot = t->init_rot;
-						t->scale = t->init_scale;
+						root_motion_cache.loc = Vector3(0, 0, 0);
+						root_motion_cache.rot = Quaternion(0, 0, 0, 1);
+						root_motion_cache.scale = Vector3(1, 1, 1);
 					}
+					t->loc = t->init_loc;
+					t->rot = t->init_rot;
+					t->scale = t->init_scale;
 				} break;
 				case Animation::TYPE_BLEND_SHAPE: {
 					TrackCacheBlendShape *t = static_cast<TrackCacheBlendShape *>(track);
@@ -1117,6 +1120,7 @@ void AnimationTree::_process_graph(double p_delta) {
 							continue; // Nothing to blend.
 						}
 						TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
+
 						if (track->root_motion && calc_root) {
 							double prev_time = time - delta;
 							if (!backward) {
@@ -1164,7 +1168,7 @@ void AnimationTree::_process_graph(double p_delta) {
 									loc[0] = post_process_key_value(a, i, loc[0], t->object, t->bone_idx);
 									a->position_track_interpolate(i, (double)a->get_length(), &loc[1]);
 									loc[1] = post_process_key_value(a, i, loc[1], t->object, t->bone_idx);
-									t->loc += (loc[1] - loc[0]) * blend;
+									root_motion_cache.loc += (loc[1] - loc[0]) * blend;
 									prev_time = 0;
 								}
 							} else {
@@ -1176,7 +1180,7 @@ void AnimationTree::_process_graph(double p_delta) {
 									loc[0] = post_process_key_value(a, i, loc[0], t->object, t->bone_idx);
 									a->position_track_interpolate(i, 0, &loc[1]);
 									loc[1] = post_process_key_value(a, i, loc[1], t->object, t->bone_idx);
-									t->loc += (loc[1] - loc[0]) * blend;
+									root_motion_cache.loc += (loc[1] - loc[0]) * blend;
 									prev_time = (double)a->get_length();
 								}
 							}
@@ -1186,13 +1190,13 @@ void AnimationTree::_process_graph(double p_delta) {
 								continue;
 							}
 							loc[0] = post_process_key_value(a, i, loc[0], t->object, t->bone_idx);
-
 							a->position_track_interpolate(i, time, &loc[1]);
 							loc[1] = post_process_key_value(a, i, loc[1], t->object, t->bone_idx);
-							t->loc += (loc[1] - loc[0]) * blend;
+							root_motion_cache.loc += (loc[1] - loc[0]) * blend;
 							prev_time = !backward ? 0 : (double)a->get_length();
+						}
 
-						} else {
+						{
 							Vector3 loc;
 
 							Error err = a->position_track_interpolate(i, time, &loc);
@@ -1211,6 +1215,7 @@ void AnimationTree::_process_graph(double p_delta) {
 							continue; // Nothing to blend.
 						}
 						TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
+
 						if (track->root_motion && calc_root) {
 							double prev_time = time - delta;
 							if (!backward) {
@@ -1258,7 +1263,7 @@ void AnimationTree::_process_graph(double p_delta) {
 									rot[0] = post_process_key_value(a, i, rot[0], t->object, t->bone_idx);
 									a->rotation_track_interpolate(i, (double)a->get_length(), &rot[1]);
 									rot[1] = post_process_key_value(a, i, rot[1], t->object, t->bone_idx);
-									t->rot = (t->rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
+									root_motion_cache.rot = (root_motion_cache.rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
 									prev_time = 0;
 								}
 							} else {
@@ -1269,7 +1274,7 @@ void AnimationTree::_process_graph(double p_delta) {
 									}
 									rot[0] = post_process_key_value(a, i, rot[0], t->object, t->bone_idx);
 									a->rotation_track_interpolate(i, 0, &rot[1]);
-									t->rot = (t->rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
+									root_motion_cache.rot = (root_motion_cache.rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
 									prev_time = (double)a->get_length();
 								}
 							}
@@ -1282,10 +1287,11 @@ void AnimationTree::_process_graph(double p_delta) {
 
 							a->rotation_track_interpolate(i, time, &rot[1]);
 							rot[1] = post_process_key_value(a, i, rot[1], t->object, t->bone_idx);
-							t->rot = (t->rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
+							root_motion_cache.rot = (root_motion_cache.rot * Quaternion().slerp(rot[0].inverse() * rot[1], blend)).normalized();
 							prev_time = !backward ? 0 : (double)a->get_length();
+						}
 
-						} else {
+						{
 							Quaternion rot;
 
 							Error err = a->rotation_track_interpolate(i, time, &rot);
@@ -1304,6 +1310,7 @@ void AnimationTree::_process_graph(double p_delta) {
 							continue; // Nothing to blend.
 						}
 						TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
+
 						if (track->root_motion && calc_root) {
 							double prev_time = time - delta;
 							if (!backward) {
@@ -1350,7 +1357,7 @@ void AnimationTree::_process_graph(double p_delta) {
 									}
 									scale[0] = post_process_key_value(a, i, scale[0], t->object, t->bone_idx);
 									a->scale_track_interpolate(i, (double)a->get_length(), &scale[1]);
-									t->scale += (scale[1] - scale[0]) * blend;
+									root_motion_cache.scale += (scale[1] - scale[0]) * blend;
 									scale[1] = post_process_key_value(a, i, scale[1], t->object, t->bone_idx);
 									prev_time = 0;
 								}
@@ -1363,7 +1370,7 @@ void AnimationTree::_process_graph(double p_delta) {
 									scale[0] = post_process_key_value(a, i, scale[0], t->object, t->bone_idx);
 									a->scale_track_interpolate(i, 0, &scale[1]);
 									scale[1] = post_process_key_value(a, i, scale[1], t->object, t->bone_idx);
-									t->scale += (scale[1] - scale[0]) * blend;
+									root_motion_cache.scale += (scale[1] - scale[0]) * blend;
 									prev_time = (double)a->get_length();
 								}
 							}
@@ -1376,10 +1383,11 @@ void AnimationTree::_process_graph(double p_delta) {
 
 							a->scale_track_interpolate(i, time, &scale[1]);
 							scale[1] = post_process_key_value(a, i, scale[1], t->object, t->bone_idx);
-							t->scale += (scale[1] - scale[0]) * blend;
+							root_motion_cache.scale += (scale[1] - scale[0]) * blend;
 							prev_time = !backward ? 0 : (double)a->get_length();
+						}
 
-						} else {
+						{
 							Vector3 scale;
 
 							Error err = a->scale_track_interpolate(i, time, &scale);
@@ -1692,10 +1700,12 @@ void AnimationTree::_process_graph(double p_delta) {
 					TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
 
 					if (t->root_motion) {
-						root_motion_position = t->loc;
-						root_motion_rotation = t->rot;
-						root_motion_scale = t->scale - Vector3(1, 1, 1);
-
+						root_motion_position = root_motion_cache.loc;
+						root_motion_rotation = root_motion_cache.rot;
+						root_motion_scale = root_motion_cache.scale - Vector3(1, 1, 1);
+						root_motion_position_accumulator = t->loc;
+						root_motion_rotation_accumulator = t->rot;
+						root_motion_scale_accumulator = t->scale;
 					} else if (t->skeleton && t->bone_idx >= 0) {
 						if (t->loc_used) {
 							t->skeleton->set_bone_pose_position(t->bone_idx, t->loc);
@@ -1997,6 +2007,18 @@ Vector3 AnimationTree::get_root_motion_scale() const {
 	return root_motion_scale;
 }
 
+Vector3 AnimationTree::get_root_motion_position_accumulator() const {
+	return root_motion_position_accumulator;
+}
+
+Quaternion AnimationTree::get_root_motion_rotation_accumulator() const {
+	return root_motion_rotation_accumulator;
+}
+
+Vector3 AnimationTree::get_root_motion_scale_accumulator() const {
+	return root_motion_scale_accumulator;
+}
+
 void AnimationTree::_tree_changed() {
 	if (properties_dirty) {
 		return;
@@ -2186,6 +2208,9 @@ void AnimationTree::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_root_motion_position"), &AnimationTree::get_root_motion_position);
 	ClassDB::bind_method(D_METHOD("get_root_motion_rotation"), &AnimationTree::get_root_motion_rotation);
 	ClassDB::bind_method(D_METHOD("get_root_motion_scale"), &AnimationTree::get_root_motion_scale);
+	ClassDB::bind_method(D_METHOD("get_root_motion_position_accumulator"), &AnimationTree::get_root_motion_position_accumulator);
+	ClassDB::bind_method(D_METHOD("get_root_motion_rotation_accumulator"), &AnimationTree::get_root_motion_rotation_accumulator);
+	ClassDB::bind_method(D_METHOD("get_root_motion_scale_accumulator"), &AnimationTree::get_root_motion_scale_accumulator);
 
 	ClassDB::bind_method(D_METHOD("_update_properties"), &AnimationTree::_update_properties);
 
diff --git a/scene/animation/animation_tree.h b/scene/animation/animation_tree.h
index 0540add85b..c68cae56ea 100644
--- a/scene/animation/animation_tree.h
+++ b/scene/animation/animation_tree.h
@@ -228,6 +228,12 @@ private:
 		}
 	};
 
+	struct RootMotionCache {
+		Vector3 loc = Vector3(0, 0, 0);
+		Quaternion rot = Quaternion(0, 0, 0, 1);
+		Vector3 scale = Vector3(1, 1, 1);
+	};
+
 	struct TrackCacheBlendShape : public TrackCache {
 		MeshInstance3D *mesh_3d = nullptr;
 		float init_value = 0;
@@ -294,6 +300,7 @@ private:
 		}
 	};
 
+	RootMotionCache root_motion_cache;
 	HashMap<NodePath, TrackCache *> track_cache;
 	HashSet<TrackCache *> playing_caches;
 	Vector<Node *> playing_audio_stream_players;
@@ -327,6 +334,9 @@ private:
 	Vector3 root_motion_position = Vector3(0, 0, 0);
 	Quaternion root_motion_rotation = Quaternion(0, 0, 0, 1);
 	Vector3 root_motion_scale = Vector3(0, 0, 0);
+	Vector3 root_motion_position_accumulator = Vector3(0, 0, 0);
+	Quaternion root_motion_rotation_accumulator = Quaternion(0, 0, 0, 1);
+	Vector3 root_motion_scale_accumulator = Vector3(1, 1, 1);
 
 	friend class AnimationNode;
 	bool properties_dirty = true;
@@ -394,6 +404,10 @@ public:
 	Quaternion get_root_motion_rotation() const;
 	Vector3 get_root_motion_scale() const;
 
+	Vector3 get_root_motion_position_accumulator() const;
+	Quaternion get_root_motion_rotation_accumulator() const;
+	Vector3 get_root_motion_scale_accumulator() const;
+
 	real_t get_connection_activity(const StringName &p_path, int p_connection) const;
 	void advance(double p_time);
 
diff --git a/scene/animation/root_motion_view.cpp b/scene/animation/root_motion_view.cpp
index 3d8d451c70..fc758b9456 100644
--- a/scene/animation/root_motion_view.cpp
+++ b/scene/animation/root_motion_view.cpp
@@ -88,6 +88,7 @@ void RootMotionView::_notification(int p_what) {
 		case NOTIFICATION_INTERNAL_PROCESS:
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
 			Transform3D transform;
+			Basis diff;
 
 			if (has_node(path)) {
 				Node *node = get_node(path);
@@ -103,9 +104,9 @@ void RootMotionView::_notification(int p_what) {
 						set_process_internal(true);
 						set_physics_process_internal(false);
 					}
-
 					transform.origin = tree->get_root_motion_position();
 					transform.basis = tree->get_root_motion_rotation(); // Scale is meaningless.
+					diff = tree->get_root_motion_rotation_accumulator();
 				}
 			}
 
@@ -115,8 +116,10 @@ void RootMotionView::_notification(int p_what) {
 
 			first = false;
 
-			accumulated.origin += transform.origin;
 			accumulated.basis *= transform.basis;
+			transform.origin = (diff.inverse() * accumulated.basis).xform(transform.origin);
+			accumulated.origin += transform.origin;
+
 			accumulated.origin.x = Math::fposmod(accumulated.origin.x, cell_size);
 			if (zero_y) {
 				accumulated.origin.y = 0;