-Add root motion support in AnimationTree.

-Add RootMotionView, to debug root motion in 3D (disabled in runtime)

4 files changed, 316 insertions, 17 deletions

diff --git a/scene/animation/animation_tree.cpp b/scene/animation/animation_tree.cpp
index 7cc67b4cc3..8f731eb51f 100644
--- a/scene/animation/animation_tree.cpp
+++ b/scene/animation/animation_tree.cpp
@@ -681,6 +681,7 @@ void AnimationTree::_clear_caches() {
 void AnimationTree::_process_graph(float p_delta) {
 
 	//check all tracks, see if they need modification
+	root_motion_transform = Transform();
 
 	if (!root.is_valid()) {
 		ERR_PRINT("AnimationTree: root AnimationNode is not set, disabling playback.");
@@ -713,6 +714,7 @@ void AnimationTree::_process_graph(float p_delta) {
 
 	{ //setup
 
+
 		process_pass++;
 
 		state.valid = true;
@@ -757,6 +759,7 @@ void AnimationTree::_process_graph(float p_delta) {
 
 			const AnimationNode::AnimationState &as = E->get();
 
+
 			Ref<Animation> a = as.animation;
 			float time = as.time;
 			float delta = as.delta;
@@ -770,6 +773,8 @@ void AnimationTree::_process_graph(float p_delta) {
 					continue; //may happen should not
 				}
 
+				track->root_motion = root_motion_track == path;
+
 				ERR_CONTINUE(!state.track_map.has(path));
 				int blend_idx = state.track_map[path];
 
@@ -786,18 +791,6 @@ void AnimationTree::_process_graph(float p_delta) {
 
 						TrackCacheTransform *t = static_cast<TrackCacheTransform *>(track);
 
-						Vector3 loc;
-						Quat rot;
-						Vector3 scale;
-
-						Error err = a->transform_track_interpolate(i, time, &loc, &rot, &scale);
-						//ERR_CONTINUE(err!=OK); //used for testing, should be removed
-
-						scale -= Vector3(1.0, 1.0, 1.0); //helps make it work properly with Add nodes
-
-						if (err != OK)
-							continue;
-
 						if (t->process_pass != process_pass) {
 
 							t->process_pass = process_pass;
@@ -806,9 +799,71 @@ void AnimationTree::_process_graph(float p_delta) {
 							t->scale = Vector3();
 						}
 
-						t->loc = t->loc.linear_interpolate(loc, blend);
-						t->rot = t->rot.slerp(rot, blend);
-						t->scale = t->scale.linear_interpolate(scale, blend);
+						if (track->root_motion) {
+
+							float prev_time = time - delta;
+							if (prev_time <0) {
+								if (!a->has_loop()) {
+									prev_time=0;
+								} else {
+									prev_time = a->get_length() + prev_time;
+								}
+							}
+
+							Vector3 loc[2];
+							Quat rot[2];
+							Vector3 scale[2];
+
+							if (prev_time > time) {
+
+								Error err = a->transform_track_interpolate(i, prev_time, &loc[0], &rot[0], &scale[0]);
+								if (err != OK) {
+									continue;
+								}
+
+								a->transform_track_interpolate(i, a->get_length(), &loc[1], &rot[1], &scale[1]);
+
+								t->loc += (loc[1] - loc[0]) * blend;
+								t->scale += (scale[1] - scale[0]) * blend;
+								Quat q = Quat().slerp(rot[0].normalized().inverse() * rot[1].normalized(),blend).normalized();
+								t->rot = (t->rot * q).normalized();
+
+								prev_time = 0;
+							}
+
+							Error err = a->transform_track_interpolate(i, prev_time, &loc[0], &rot[0], &scale[0]);
+							if (err != OK) {
+								continue;
+							}
+
+							a->transform_track_interpolate(i, time, &loc[1], &rot[1], &scale[1]);
+
+							t->loc += (loc[1] - loc[0]) * blend;
+							t->scale += (scale[1] - scale[0]) * blend;
+							Quat q = Quat().slerp(rot[0].normalized().inverse() * rot[1].normalized(),blend).normalized();
+							t->rot = (t->rot * q).normalized();
+
+							prev_time = 0;
+
+
+
+						} else {
+							Vector3 loc;
+							Quat rot;
+							Vector3 scale;
+
+							Error err = a->transform_track_interpolate(i, time, &loc, &rot, &scale);
+							//ERR_CONTINUE(err!=OK); //used for testing, should be removed
+
+							scale -= Vector3(1.0, 1.0, 1.0); //helps make it work properly with Add nodes
+
+							if (err != OK)
+								continue;
+
+							t->loc = t->loc.linear_interpolate(loc, blend);
+							t->rot = t->rot.slerp(rot, blend);
+							t->scale = t->scale.linear_interpolate(scale, blend);
+						}
 
 					} break;
 					case Animation::TYPE_VALUE: {
@@ -1059,11 +1114,18 @@ void AnimationTree::_process_graph(float p_delta) {
 					Transform xform;
 					xform.origin = t->loc;
 
-					t->scale += Vector3(1.0, 1.0, 1.0); //helps make it work properly with Add nodes
+					t->scale += Vector3(1.0, 1.0, 1.0); //helps make it work properly with Add nodes and root motion
 
 					xform.basis.set_quat_scale(t->rot, t->scale);
 
-					if (t->skeleton && t->bone_idx >= 0) {
+					if (t->root_motion) {
+
+						root_motion_transform = xform;
+
+						if (t->skeleton && t->bone_idx >= 0) {
+							root_motion_transform = (t->skeleton->get_bone_rest(t->bone_idx) * root_motion_transform) *t->skeleton->get_bone_rest(t->bone_idx).affine_inverse();
+						}
+					} else 	if (t->skeleton && t->bone_idx >= 0) {
 
 						t->skeleton->set_bone_pose(t->bone_idx, xform);
 
@@ -1174,6 +1236,19 @@ String AnimationTree::get_configuration_warning() const {
 	return warning;
 }
 
+void AnimationTree::set_root_motion_track(const NodePath& p_track) {
+	root_motion_track=p_track;
+}
+
+NodePath AnimationTree::get_root_motion_track() const {
+	return root_motion_track;
+}
+
+
+Transform AnimationTree::get_root_motion_transform() const {
+	return root_motion_transform;
+}
+
 void AnimationTree::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_active", "active"), &AnimationTree::set_active);
 	ClassDB::bind_method(D_METHOD("is_active"), &AnimationTree::is_active);
@@ -1187,12 +1262,19 @@ void AnimationTree::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_animation_player", "root"), &AnimationTree::set_animation_player);
 	ClassDB::bind_method(D_METHOD("get_animation_player"), &AnimationTree::get_animation_player);
 
+	ClassDB::bind_method(D_METHOD("set_root_motion_track", "path"), &AnimationTree::set_root_motion_track);
+	ClassDB::bind_method(D_METHOD("get_root_motion_track"), &AnimationTree::get_root_motion_track);
+
 	ClassDB::bind_method(D_METHOD("_node_removed"), &AnimationTree::_node_removed);
 
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "graph_root", PROPERTY_HINT_RESOURCE_TYPE, "AnimationRootNode", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_DO_NOT_SHARE_ON_DUPLICATE), "set_graph_root", "get_graph_root");
 	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "anim_player"), "set_animation_player", "get_animation_player");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "active"), "set_active", "is_active");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "process_mode", PROPERTY_HINT_ENUM, "Physics,Idle"), "set_process_mode", "get_process_mode");
+	ADD_GROUP("Root Motion","root_motion_");
+	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "root_motion_track"),"set_root_motion_track", "get_root_motion_track");
+
+
 }
 
 AnimationTree::AnimationTree() {
diff --git a/scene/animation/animation_tree.h b/scene/animation/animation_tree.h
index adc25d5607..22fb309578 100644
--- a/scene/animation/animation_tree.h
+++ b/scene/animation/animation_tree.h
@@ -135,6 +135,8 @@ public:
 
 private:
 	struct TrackCache {
+
+		bool root_motion;
 		uint64_t setup_pass;
 		uint64_t process_pass;
 		Animation::TrackType type;
@@ -142,6 +144,7 @@ private:
 		ObjectID object_id;
 
 		TrackCache() {
+			root_motion = false;
 			setup_pass = 0;
 			process_pass = 0;
 			object = NULL;
@@ -235,6 +238,9 @@ private:
 
 	bool started;
 
+	NodePath root_motion_track;
+	Transform root_motion_transform;
+
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
@@ -257,6 +263,11 @@ public:
 	bool is_state_invalid() const;
 	String get_invalid_state_reason() const;
 
+	void set_root_motion_track(const NodePath &p_track);
+	NodePath get_root_motion_track() const;
+
+	Transform get_root_motion_transform() const;
+
 	uint64_t get_last_process_pass() const;
 	AnimationTree();
 	~AnimationTree();
diff --git a/scene/animation/root_motion_view.cpp b/scene/animation/root_motion_view.cpp
new file mode 100644
index 0000000000..1625b80db4
--- /dev/null
+++ b/scene/animation/root_motion_view.cpp
@@ -0,0 +1,163 @@
+#include "root_motion_view.h"
+#include "scene/animation/animation_tree.h"
+#include "scene/resources/material.h"
+void RootMotionView::set_animation_path(const NodePath &p_path) {
+	path = p_path;
+	first = true;
+}
+
+NodePath RootMotionView::get_animation_path() const {
+	return path;
+}
+
+void RootMotionView::set_color(const Color &p_color) {
+	color = p_color;
+	first = true;
+}
+
+Color RootMotionView::get_color() const {
+	return color;
+}
+
+void RootMotionView::set_cell_size(float p_size) {
+	cell_size = p_size;
+	first = true;
+}
+
+float RootMotionView::get_cell_size() const {
+	return cell_size;
+}
+
+void RootMotionView::set_radius(float p_radius) {
+	radius = p_radius;
+	first = true;
+}
+
+float RootMotionView::get_radius() const {
+	return radius;
+}
+
+void RootMotionView::_notification(int p_what) {
+
+	if (p_what == NOTIFICATION_ENTER_TREE) {
+
+		VS::get_singleton()->immediate_set_material(immediate, SpatialMaterial::get_material_rid_for_2d(false, true, false, false, false));
+		first = true;
+	}
+
+	if (p_what == NOTIFICATION_INTERNAL_PROCESS || p_what == NOTIFICATION_INTERNAL_PHYSICS_PROCESS) {
+		Transform transform;
+
+		if (has_node(path)) {
+
+			Node *node = get_node(path);
+
+			AnimationTree *tree = Object::cast_to<AnimationTree>(node);
+			if (tree && tree->is_active() && tree->get_root_motion_track() != NodePath()) {
+				if (is_processing_internal() && tree->get_process_mode() == AnimationTree::ANIMATION_PROCESS_PHYSICS) {
+					set_process_internal(false);
+					set_physics_process_internal(true);
+				}
+
+				if (is_physics_processing_internal() && tree->get_process_mode() == AnimationTree::ANIMATION_PROCESS_IDLE) {
+					set_process_internal(true);
+					set_physics_process_internal(false);
+				}
+
+				transform = tree->get_root_motion_transform();
+			}
+		}
+
+		if (!first && transform == Transform()) {
+			return;
+		}
+
+		first = false;
+
+		transform.orthonormalize(); //dont want scale, too imprecise
+		transform.affine_invert();
+
+		accumulated = accumulated * transform;
+		accumulated.origin.x = Math::fposmod(accumulated.origin.x, cell_size);
+		accumulated.origin.y = Math::fposmod(accumulated.origin.y, cell_size);
+		accumulated.origin.z = Math::fposmod(accumulated.origin.z, cell_size);
+
+		VS::get_singleton()->immediate_clear(immediate);
+
+		int cells_in_radius = int((radius / cell_size) + 1.0);
+
+		VS::get_singleton()->immediate_begin(immediate, VS::PRIMITIVE_LINES);
+		for (int i = -cells_in_radius; i < cells_in_radius; i++) {
+			for (int j = -cells_in_radius; j < cells_in_radius; j++) {
+
+				Vector3 from(i * cell_size, 0, j * cell_size);
+				Vector3 from_i((i + 1) * cell_size, 0, j * cell_size);
+				Vector3 from_j(i * cell_size, 0, (j + 1) * cell_size);
+				from = accumulated.xform(from);
+				from_i = accumulated.xform(from_i);
+				from_j = accumulated.xform(from_j);
+
+				Color c = color, c_i = color, c_j = color;
+				c.a *= MAX(0, 1.0 - from.length() / radius);
+				c_i.a *= MAX(0, 1.0 - from_i.length() / radius);
+				c_j.a *= MAX(0, 1.0 - from_j.length() / radius);
+
+				VS::get_singleton()->immediate_color(immediate, c);
+				VS::get_singleton()->immediate_vertex(immediate, from);
+
+				VS::get_singleton()->immediate_color(immediate, c_i);
+				VS::get_singleton()->immediate_vertex(immediate, from_i);
+
+				VS::get_singleton()->immediate_color(immediate, c);
+				VS::get_singleton()->immediate_vertex(immediate, from);
+
+				VS::get_singleton()->immediate_color(immediate, c_j);
+				VS::get_singleton()->immediate_vertex(immediate, from_j);
+			}
+		}
+
+		VS::get_singleton()->immediate_end(immediate);
+	}
+}
+
+AABB RootMotionView::get_aabb() const {
+
+	return AABB(Vector3(-radius, 0, -radius), Vector3(radius * 2, 0.001, radius * 2));
+}
+PoolVector<Face3> RootMotionView::get_faces(uint32_t p_usage_flags) const {
+	return PoolVector<Face3>();
+}
+
+void RootMotionView::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_animation_path", "path"), &RootMotionView::set_animation_path);
+	ClassDB::bind_method(D_METHOD("get_animation_path"), &RootMotionView::get_animation_path);
+
+	ClassDB::bind_method(D_METHOD("set_color", "color"), &RootMotionView::set_color);
+	ClassDB::bind_method(D_METHOD("get_color"), &RootMotionView::get_color);
+
+	ClassDB::bind_method(D_METHOD("set_cell_size", "size"), &RootMotionView::set_cell_size);
+	ClassDB::bind_method(D_METHOD("get_cell_size"), &RootMotionView::get_cell_size);
+
+	ClassDB::bind_method(D_METHOD("set_radius", "size"), &RootMotionView::set_radius);
+	ClassDB::bind_method(D_METHOD("get_radius"), &RootMotionView::get_radius);
+
+	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "animation_path"), "set_animation_path", "get_animation_path");
+	ADD_PROPERTY(PropertyInfo(Variant::COLOR, "color"), "set_color", "get_color");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_size", PROPERTY_HINT_RANGE, "0.1,16,0.01,or_greater"), "set_cell_size", "get_cell_size");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.1,16,0.01,or_greater"), "set_radius", "get_radius");
+}
+
+RootMotionView::RootMotionView() {
+	radius = 10;
+	cell_size = 1;
+	set_process_internal(true);
+	immediate = VisualServer::get_singleton()->immediate_create();
+	set_base(immediate);
+	color = Color(0.5, 0.5, 1.0);
+}
+
+RootMotionView::~RootMotionView() {
+	set_base(RID());
+	VisualServer::get_singleton()->free(immediate);
+}
diff --git a/scene/animation/root_motion_view.h b/scene/animation/root_motion_view.h
new file mode 100644
index 0000000000..65e9ff480b
--- /dev/null
+++ b/scene/animation/root_motion_view.h
@@ -0,0 +1,43 @@
+#ifndef ROOT_MOTION_VIEW_H
+#define ROOT_MOTION_VIEW_H
+
+#include "scene/3d/visual_instance.h"
+
+class RootMotionView : public VisualInstance {
+	GDCLASS(RootMotionView, VisualInstance)
+public:
+	RID immediate;
+	NodePath path;
+	float cell_size;
+	float radius;
+	bool use_in_game;
+	Color color;
+	bool first;
+
+	Transform accumulated;
+
+private:
+	void _notification(int p_what);
+	static void _bind_methods();
+
+public:
+	void set_animation_path(const NodePath &p_path);
+	NodePath get_animation_path() const;
+
+	void set_color(const Color &p_path);
+	Color get_color() const;
+
+	void set_cell_size(float p_size);
+	float get_cell_size() const;
+
+	void set_radius(float p_radius);
+	float get_radius() const;
+
+	virtual AABB get_aabb() const;
+	virtual PoolVector<Face3> get_faces(uint32_t p_usage_flags) const;
+
+	RootMotionView();
+	~RootMotionView();
+};
+
+#endif // ROOT_MOTION_VIEW_H