HEADmirror

1 files changed, 99 insertions, 17 deletions

diff --git a/scene/animation/animation_tree.cpp b/scene/animation/animation_tree.cpp
index 7cc67b4cc..8f731eb51 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() {