diff --git a/modules/motionmatch/SCsub b/modules/motionmatch/SCsub new file mode 100644 index 000000000000..37dab7e8c623 --- /dev/null +++ b/modules/motionmatch/SCsub @@ -0,0 +1,9 @@ +#!/usr/bin/env python + +Import('env') +Import('env_modules') + +env_mm = env_modules.Clone() + +# Godot source files +env_mm.add_source_files(env.modules_sources, "*.cpp") diff --git a/modules/motionmatch/animation_motion_match_editor.cpp b/modules/motionmatch/animation_motion_match_editor.cpp new file mode 100644 index 000000000000..1dccc313d8a6 --- /dev/null +++ b/modules/motionmatch/animation_motion_match_editor.cpp @@ -0,0 +1,426 @@ +#include "animation_motion_match_editor.h" +#include +#include + +#ifdef TOOLS_ENABLED +#include "core/io/resource_loader.h" + +bool AnimationNodeMotionMatchEditor::can_edit( + const Ref &p_node) { + + Ref anmm = p_node; + return anmm.is_valid(); +} + +void AnimationNodeMotionMatchEditor::edit(const Ref &p_node) { + + motion_match = p_node; + + if (motion_match.is_valid()) { + } +} + +void AnimationNodeMotionMatchEditor::_match_tracks_edited() { + if (updating) { + return; + } + + TreeItem *edited = match_tracks->get_edited(); + ERR_FAIL_COND(!edited); + + NodePath edited_path = edited->get_metadata(0); + bool matched = edited->is_checked(0); + + UndoRedo *undo_redo = EditorNode::get_singleton()->get_undo_redo(); + + updating = true; + undo_redo->create_action(TTR("Change Match Track")); + if (matched) { + undo_redo->add_do_method(motion_match.ptr(), "add_matching_track", + edited_path); + undo_redo->add_undo_method(motion_match.ptr(), "remove_matching_track", + edited_path); + } else { + undo_redo->add_do_method(motion_match.ptr(), "remove_matching_track", + edited_path); + undo_redo->add_undo_method(motion_match.ptr(), "add_matching_track", + edited_path); + } + undo_redo->add_do_method(this, "_update_match_tracks"); + undo_redo->add_undo_method(this, "_update_match_tracks"); + undo_redo->commit_action(); + updating = false; +} + +void AnimationNodeMotionMatchEditor::_edit_match_tracks() { + + _update_match_tracks(); + motion_match->editing = true; + match_tracks_dialog->popup_centered_minsize(Size2(500, 500) * EDSCALE); +} + +void AnimationNodeMotionMatchEditor::_clear_tree() { + motion_match->clear_keys(); +} + +void AnimationNodeMotionMatchEditor::_update_match_tracks() { + + if (!is_visible()) { + return; + } + + if (updating) { + return; + } + + /*Checking for errors*/ + + NodePath player_path = + AnimationTreeEditor::get_singleton()->get_tree()->get_animation_player(); + if (!AnimationTreeEditor::get_singleton()->get_tree()->has_node( + player_path)) { + EditorNode::get_singleton()->show_warning( + TTR("No animation player set, so unable to retrieve track names.")); + return; + } + + AnimationPlayer *player = Object::cast_to( + AnimationTreeEditor::get_singleton()->get_tree()->get_node(player_path)); + if (!player) { + EditorNode::get_singleton()->show_warning( + TTR("Player path set is invalid, so unable to retrieve track names.")); + return; + } + + Node *base = player->get_node(player->get_root()); + if (!base) { + EditorNode::get_singleton()->show_warning( + TTR("Animation player has no valid root node path, so unable to " + "retrieve track names.")); + return; + } + /**/ + + /*Get the list of all bones and display it*/ + updating = true; + + Set paths; + List animations; + player->get_animation_list(&animations); + + for (List::Element *E = animations.front(); E; E = E->next()) { + + Ref anim = player->get_animation(E->get()); + for (int i = 0; i < anim->get_track_count(); i++) { + paths.insert(anim->track_get_path(i)); + } + } + + match_tracks->clear(); + TreeItem *root = match_tracks->create_item(); + + Map parenthood; + + for (Set::Element *E = paths.front(); E; E = E->next()) { + NodePath path = E->get(); + TreeItem *ti = NULL; + String accum; + for (int i = 0; i < path.get_name_count(); i++) { + String name = path.get_name(i); + if (accum != String()) { + accum += "/"; + } + accum += name; + if (!parenthood.has(accum)) { + if (ti) { + ti = match_tracks->create_item(ti); + } else { + ti = match_tracks->create_item(root); + } + parenthood[accum] = ti; + ti->set_text(0, name); + ti->set_selectable(0, false); + ti->set_editable(0, false); + if (base->has_node(accum)) { + Node *node = base->get_node(accum); + ti->set_icon( + 0, EditorNode::get_singleton()->get_object_icon(node, "Node")); + } + + } else { + ti = parenthood[accum]; + } + } + Node *node = NULL; + if (base->has_node(accum)) { + node = base->get_node(accum); + } + if (!node) + continue; // no node, can't edit + + if (path.get_subname_count()) { + + String concat = path.get_concatenated_subnames(); + this->skeleton = Object::cast_to(node); + if (skeleton && skeleton->find_bone(concat) != -1) { + // path in skeleton + String bone = concat; + int idx = skeleton->find_bone(bone); + List bone_path; + while (idx != -1) { + bone_path.push_front(skeleton->get_bone_name(idx)); + idx = skeleton->get_bone_parent(idx); + } + + accum += ":"; + for (List::Element *F = bone_path.front(); F; F = F->next()) { + if (F != bone_path.front()) { + accum += "/"; + } + + accum += F->get(); + if (!parenthood.has(accum)) { + ti = match_tracks->create_item(ti); + parenthood[accum] = ti; + ti->set_text(0, F->get()); + ti->set_selectable(0, false); + ti->set_editable(0, false); + ti->set_icon(0, get_icon("BoneAttachment", "EditorIcons")); + } else { + ti = parenthood[accum]; + } + } + + ti->set_editable(0, true); + ti->set_selectable(0, true); + ti->set_cell_mode(0, TreeItem::CELL_MODE_CHECK); + ti->set_text(0, concat); + ti->set_checked(0, motion_match->is_matching_track(path)); + ti->set_icon(0, get_icon("BoneAttachment", "EditorIcons")); + ti->set_metadata(0, path); + + } else { + // just a property + ti = match_tracks->create_item(ti); + ti->set_cell_mode(0, TreeItem::CELL_MODE_CHECK); + ti->set_text(0, concat); + ti->set_editable(0, true); + ti->set_selectable(0, true); + ti->set_checked(0, motion_match->is_matching_track(path)); + ti->set_metadata(0, path); + } + } else { + if (ti) { + // just a node, likely call or animation track + ti->set_editable(0, true); + ti->set_selectable(0, true); + ti->set_cell_mode(0, TreeItem::CELL_MODE_CHECK); + ti->set_checked(0, motion_match->is_matching_track(path)); + ti->set_metadata(0, path); + } + } + } + /**/ + updating = false; +} + +void AnimationNodeMotionMatchEditor::_update_tracks() { + print_line(itos(motion_match->get_matching_tracks().size())); + if (motion_match->get_matching_tracks().size() == 0) { + EditorNode::get_singleton()->show_warning( + TTR("Please select tracks to match!")); + } + NodePath player_path = + AnimationTreeEditor::get_singleton()->get_tree()->get_animation_player(); + /*Checking for errors*/ + if (!AnimationTreeEditor::get_singleton()->get_tree()->has_node( + player_path)) { + EditorNode::get_singleton()->show_warning( + TTR("No animation player set, so unable to retrieve track names.")); + return; + } + + AnimationPlayer *player = Object::cast_to( + AnimationTreeEditor::get_singleton()->get_tree()->get_node(player_path)); + if (!player) { + EditorNode::get_singleton()->show_warning( + TTR("Player path set is invalid, so unable to retrieve track names.")); + return; + } + + if ((AnimationTreeEditor::get_singleton() + ->get_tree() + ->get_root_motion_track() == NodePath())) { + EditorNode::get_singleton()->show_warning( + TTR("No root motion track was set, unable to build database.")); + return; + } + + /**/ + /*UPDATING DATABASE*/ + + motion_match->clear_keys(); + motion_match->set_dim_len(9); + List Animations; + player->get_animation_list(&Animations); + const StringName t = "Tracker"; + for (int i = 0; i < Animations.size(); i++) { + if (Animations[i] != t) { + Ref anim = player->get_animation(Animations[i]); + int root = anim->find_track(AnimationTreeEditor::get_singleton() + ->get_tree() + ->get_root_motion_track()); + NodePath root_motion_track = AnimationTreeEditor::get_singleton()->get_tree()->get_root_motion_track(); + int max_count = + fill_tracks(player, anim.ptr(), root_motion_track); + + motion_match->delta_time = + anim->track_get_key_time(max_key_track, max_count - 1) / max_count; + + for (int j = 0; j < max_count - snap_x->get_value(); j++) { + float x = 0; + float z = 0; + for (int p = 0; p < 2; p++) { + x += Math::pow(-1.0, double(p + 1)) * + Vector3(Dictionary(anim->track_get_key_value(root, j + p)) + .get("location", Variant()))[0]; + z += Math::pow(-1.0, double(p + 1)) * + Vector3(Dictionary(anim->track_get_key_value(root, j + p)) + .get("location", Variant()))[2]; + } + + frame_model *key = new frame_model; + for (int y = 0; y < motion_match->get_matching_tracks().size(); y++) { + int track = anim->find_track(motion_match->get_matching_tracks()[y]); + Vector3 loc = Vector3(Dictionary(anim->track_get_key_value(track, j)) + .get("location", Variant())); + PoolRealArray arr = {}; + for (int l = 0; l < snap_x->get_value(); l++) { + if (l != 1) { + arr.append(loc[l]); + } + } + key->bone_data->append(arr); + } + Vector3 r_loc = Vector3(Dictionary(anim->track_get_key_value(root, j)) + .get("location", Variant())); + for (int k = 0; k < snap_x->get_value(); k++) { + Vector3 loc = + Vector3(Dictionary(anim->track_get_key_value(root, j + k)) + .get("location", Variant())); + for (int l = 0; l < 3; l++) { + if (l != 1) { + key->traj.append((loc[l] - r_loc[l]) * 10); + } + } + } + + key->time = anim->track_get_key_time(root, j + 1); + key->anim_num = i; + keys->append(key); + } + } + } + motion_match->editing = false; + motion_match->set_keys_data(keys); + motion_match->skeleton = skeleton; + if (motion_match->get_matching_tracks().size() != 0) { + + motion_match->done = true; + } + print_line("DONE"); + /**/ +} + +void AnimationNodeMotionMatchEditor::_bind_methods() { + ClassDB::bind_method("_match_tracks_edited", + &AnimationNodeMotionMatchEditor::_match_tracks_edited); + ClassDB::bind_method("_update_match_tracks", + &AnimationNodeMotionMatchEditor::_update_match_tracks); + ClassDB::bind_method("_edit_match_tracks", + &AnimationNodeMotionMatchEditor::_edit_match_tracks); + ClassDB::bind_method("_update_tracks", + &AnimationNodeMotionMatchEditor::_update_tracks); + ClassDB::bind_method("_clear_tree", + &AnimationNodeMotionMatchEditor::_clear_tree); +} +AnimationNodeMotionMatchEditor::AnimationNodeMotionMatchEditor() { + + match_tracks_dialog = memnew(AcceptDialog); + add_child(match_tracks_dialog); + match_tracks_dialog->set_title(TTR("Tracks to Match:")); + + VBoxContainer *match_tracks_vbox = memnew(VBoxContainer); + match_tracks_dialog->add_child(match_tracks_vbox); + + match_tracks = memnew(Tree); + match_tracks_vbox->add_child(match_tracks); + match_tracks->set_v_size_flags(SIZE_EXPAND_FILL); + match_tracks->set_hide_root(true); + match_tracks->connect("item_edited", this, "_match_tracks_edited"); + + edit_match_tracks = memnew(Button("Edit Matching Tracks")); + add_child(edit_match_tracks); + edit_match_tracks->connect("pressed", this, "_edit_match_tracks"); + + snap_x = memnew(SpinBox); + add_child(snap_x); + snap_x->set_prefix("Samples:"); + snap_x->set_min(1); + snap_x->set_step(1); + snap_x->set_max(100); + + update_tracks = memnew(Button("Update DataBase")); + add_child(update_tracks); + update_tracks->connect("pressed", this, "_update_tracks"); + + HBoxContainer *velocity_vbox = memnew(HBoxContainer); + Label *l = memnew(Label); + + l->set_text("Velocity"); + velocity_vbox->add_child(l); + + updating = false; +} + +int AnimationNodeMotionMatchEditor::fill_tracks(AnimationPlayer *player, + Animation *anim, + NodePath &root) { + int max_keys = 0; + Vector tracks_tf = motion_match->get_matching_tracks(); + tracks_tf.push_back(root); + for (int i = 0; i < tracks_tf.size(); i++) { + if (anim->track_get_key_count(anim->find_track(tracks_tf[i])) > + anim->track_get_key_count(anim->find_track(tracks_tf[max_keys]))) { + max_keys = i; + } + } + for (int i = 0; + i < anim->track_get_key_count(anim->find_track(tracks_tf[max_keys])); + i++) { + float min_time = + anim->track_get_key_time(anim->find_track(tracks_tf[0]), i); + for (int p = 0; p < tracks_tf.size(); p++) { + if (anim->track_get_key_time(anim->find_track(tracks_tf[p]), i) < + min_time) { + min_time = anim->track_get_key_time(anim->find_track(tracks_tf[p]), i); + } + } + for (int p = 0; p < tracks_tf.size(); p++) { + if (anim->track_get_key_time(anim->find_track(tracks_tf[p]), i) > + min_time) { + Vector3 t1; + Quat t2; + Vector3 t3; + anim->transform_track_interpolate(anim->find_track(tracks_tf[p]), + min_time, &t1, &t2, &t3); + anim->transform_track_insert_key(anim->find_track(tracks_tf[p]), + min_time, t1, t2, t3); + } + } + } + max_key_track = anim->find_track(tracks_tf[max_keys]); + return anim->track_get_key_count(anim->find_track(tracks_tf[max_keys])); +} + +#endif diff --git a/modules/motionmatch/animation_motion_match_editor.h b/modules/motionmatch/animation_motion_match_editor.h new file mode 100644 index 000000000000..ce656e021e3a --- /dev/null +++ b/modules/motionmatch/animation_motion_match_editor.h @@ -0,0 +1,53 @@ +#ifndef ANIMATION_MOTION_MATCH_EDITOR_H +#define ANIMATION_MOTION_MATCH_EDITOR_H + +#ifdef TOOLS_ENABLED + +#include "frame_model.h" + +#include "core/reference.h" +#include "editor/plugins/animation_tree_editor_plugin.h" +#include "modules/motionmatch/animation_node_motion_match.h" + +class AnimationNodeMotionMatchEditor : public AnimationTreeNodeEditorPlugin { + + GDCLASS(AnimationNodeMotionMatchEditor, AnimationTreeNodeEditorPlugin) + + Ref motion_match; + + AcceptDialog *match_tracks_dialog; + Tree *match_tracks; + + Button *edit_match_tracks; + Button *update_tracks; + Button *clear_tree; + + SpinBox *snap_x; + + Skeleton *skeleton; + + bool updating; + PoolVector *keys = new PoolVector(); + + void _match_tracks_edited(); + + void _edit_match_tracks(); + void _update_match_tracks(); + void _update_tracks(); + void _clear_tree(); + int max_key_track; + void _update_vel(); + +protected: + static void _bind_methods(); + +public: + virtual bool can_edit(const Ref &p_node); + virtual void edit(const Ref &p_node); + + int fill_tracks(AnimationPlayer *player, Animation *anim, NodePath &root); + + AnimationNodeMotionMatchEditor(); +}; +#endif // ANIMATION_MOTION_MATCH_EDITOR_H +#endif diff --git a/modules/motionmatch/animation_node_motion_match.cpp b/modules/motionmatch/animation_node_motion_match.cpp new file mode 100644 index 000000000000..f5b7533d2999 --- /dev/null +++ b/modules/motionmatch/animation_node_motion_match.cpp @@ -0,0 +1,583 @@ +#include "animation_node_motion_match.h" +#include "scene/main/node.h" + +void AnimationNodeMotionMatch::get_parameter_list( + List *r_list) const { + r_list->push_back(PropertyInfo(Variant::INT, min, PROPERTY_HINT_NONE, "")); + r_list->push_back(PropertyInfo(Variant::INT, samples, PROPERTY_HINT_RANGE, + "5,40,1,or_greater")); + r_list->push_back(PropertyInfo(Variant::REAL, pvst, PROPERTY_HINT_RANGE, + "0,1,0.01,or_greater")); + r_list->push_back(PropertyInfo(Variant::REAL, f_time, PROPERTY_HINT_RANGE, + "0,2,0.01,or_greater")); +} + +Variant AnimationNodeMotionMatch::get_parameter_default_value( + const StringName &p_parameter) const { + if (p_parameter == min) { + return 0; + } else if (p_parameter == samples) { + return 10; + } else if (p_parameter == pvst) { + return 0.5; + } else if (p_parameter == f_time) { + return 0.25; + } else { + return Variant(); + } +} + +void AnimationNodeMotionMatch::add_matching_track( + const NodePath &p_track_path) { + matching_tracks.push_back(p_track_path); +} // Adds tracks to matching_tracks + +void AnimationNodeMotionMatch::remove_matching_track( + const NodePath &p_track_path) { + + matching_tracks.erase(p_track_path); +} // removes tracks + +bool AnimationNodeMotionMatch::is_matching_track( + const NodePath &p_track_path) const { + return matching_tracks.find(p_track_path) != -1; +} + +void AnimationNodeMotionMatch::update_motion_database( + AnimationPlayer *p_animation_player) { + for (int i = 0; i < matching_tracks.size(); i++) { + print_line("track " + itos(i) + ": " + String(matching_tracks[i])); + } +} + +Vector AnimationNodeMotionMatch::get_matching_tracks() { + return matching_tracks; +} + +void AnimationNodeMotionMatch::_bind_methods() { + + ClassDB::bind_method(D_METHOD("add_matching_track", "track"), + &AnimationNodeMotionMatch::add_matching_track); + ClassDB::bind_method(D_METHOD("remove_matching_track", "track"), + &AnimationNodeMotionMatch::remove_matching_track); + ClassDB::bind_method(D_METHOD("is_matching_track", "track"), + &AnimationNodeMotionMatch::is_matching_track); + + ClassDB::bind_method(D_METHOD("add_coordinates", "point"), + &AnimationNodeMotionMatch::add_coordinates); + ClassDB::bind_method(D_METHOD("load_coordinates", "points"), + &AnimationNodeMotionMatch::load_coordinates); + ClassDB::bind_method(D_METHOD("clear_coordinates"), + &AnimationNodeMotionMatch::clear_coordinates); + ClassDB::bind_method(D_METHOD("get_coordinates"), + &AnimationNodeMotionMatch::get_coordinates); + + ClassDB::bind_method(D_METHOD("set_dim_len", "dim_len"), + &AnimationNodeMotionMatch::set_dim_len); + ClassDB::bind_method(D_METHOD("get_dim_len"), + &AnimationNodeMotionMatch::get_dim_len); + + ClassDB::bind_method(D_METHOD("set_min_leaves", "min_leaves"), + &AnimationNodeMotionMatch::set_min_leaves); + ClassDB::bind_method(D_METHOD("get_min_leaves"), + &AnimationNodeMotionMatch::get_min_leaves); + + ClassDB::bind_method(D_METHOD("get_root"), + &AnimationNodeMotionMatch::get_root); + ClassDB::bind_method(D_METHOD("build_tree"), + &AnimationNodeMotionMatch::build_tree); + + ClassDB::bind_method(D_METHOD("set_start_index", "si"), + &AnimationNodeMotionMatch::set_start_index); + ClassDB::bind_method(D_METHOD("get_start_index"), + &AnimationNodeMotionMatch::get_start_index); + + ClassDB::bind_method(D_METHOD("calc_root_threshold"), + &AnimationNodeMotionMatch::calc_root_threshold); + ClassDB::bind_method(D_METHOD("KNNSearch", "point", "k"), + &AnimationNodeMotionMatch::KNNSearch); + + ClassDB::bind_method(D_METHOD("calculate_threshold", "point_coordinates"), + &AnimationNodeMotionMatch::KDNode::calculate_threshold); + + ClassDB::bind_method(D_METHOD("add_index", "ind"), + &AnimationNodeMotionMatch::KDNode::add_index); + ClassDB::bind_method(D_METHOD("clear_indices"), + &AnimationNodeMotionMatch::KDNode::clear_indices); + ClassDB::bind_method(D_METHOD("get_indices"), + &AnimationNodeMotionMatch::KDNode::get_indices); + + ClassDB::bind_method(D_METHOD("leaf_split", "point_coordinates_data", + "no_of_dims", "start_dim"), + &AnimationNodeMotionMatch::KDNode::leaf_split); + + ClassDB::bind_method(D_METHOD("get_left"), + &AnimationNodeMotionMatch::KDNode::get_left); + ClassDB::bind_method(D_METHOD("get_right"), + &AnimationNodeMotionMatch::KDNode::get_right); + + ClassDB::bind_method(D_METHOD("get_prev"), + &AnimationNodeMotionMatch::KDNode::get_prev); + + ClassDB::bind_method(D_METHOD("set_th", "th"), + &AnimationNodeMotionMatch::KDNode::set_th); + ClassDB::bind_method(D_METHOD("get_th"), + &AnimationNodeMotionMatch::KDNode::get_th); + + ClassDB::bind_method(D_METHOD("Predict_traj", "vel", "samples"), + &AnimationNodeMotionMatch::Predict_traj); + + ClassDB::bind_method(D_METHOD("set_velocity", "vel"), + &AnimationNodeMotionMatch::set_velocity); + ClassDB::bind_method(D_METHOD("get_velocity"), + &AnimationNodeMotionMatch::get_velocity); + + // for trajectory drawing + + ClassDB::bind_method(D_METHOD("get_keys_size"), + &AnimationNodeMotionMatch::get_key_size); + + ClassDB::bind_method(D_METHOD("get_key_traj", "key number"), + &AnimationNodeMotionMatch::get_key_traj); + + ClassDB::bind_method(D_METHOD("get_future_traj"), + &AnimationNodeMotionMatch::get_future_traj); + + ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "velocity"), "set_velocity", + "get_velocity"); +} + +AnimationNodeMotionMatch::AnimationNodeMotionMatch() { + root.instance(); + dim_len = 2; + start_index = 0; + point_coordinates = {}; + min_leaves = 1; + + this->pos = "position"; + this->min = "min_key"; + this->pvst = "Pose vs Trajectory"; + this->f_time = "Check rate"; + this->samples = "Future Traj Samples"; +} + +AnimationNodeMotionMatch::~AnimationNodeMotionMatch() {} + +void AnimationNodeMotionMatch::add_coordinates(PoolRealArray point) { + if (point.size() != dim_len) { + print_line("ERROR: Point is of wrong size."); + error = true; + err = LOAD_POINT_ERROR; + } else { + for (int i = 0; i < point.size(); i++) { + this->point_coordinates.append(point[i]); + } + root->add_index(root->get_indices().size()); + } +} // adds point to KD-tree + +void AnimationNodeMotionMatch::load_coordinates(PoolRealArray points) { + if (dim_len < 0) { + print_line("ERROR: Length is negative"); + error = true; + err = LOAD_POINT_ERROR; + } else if (points.size() % dim_len != 0) { + print_line("ERROR: Point is of wrong size."); + error = true; + err = LOAD_POINT_ERROR; + } else { + for (int i = 0; i < points.size() / dim_len; i++) { + PoolRealArray point; + for (int j = 0; j < dim_len; j++) { + point.append(points[i * dim_len + j]); + } + add_coordinates(point); + } + } +} // load multiple points at a go to KD-Tree + +PoolRealArray AnimationNodeMotionMatch::get_coordinates() { + return this->point_coordinates; +} + +void AnimationNodeMotionMatch::clear_coordinates() { + this->point_coordinates.resize(0); +} + +void AnimationNodeMotionMatch::set_dim_len(int32_t dim_len) { + this->dim_len = dim_len; +} // set dimension length + +int32_t AnimationNodeMotionMatch::get_dim_len() { + return this->dim_len; +} + +AnimationNodeMotionMatch::KDNode *AnimationNodeMotionMatch::get_root() { + return root.ptr(); +} // returns root of the KDTree + +void AnimationNodeMotionMatch::clear_root() { + root->point_indices = {}; + root->split_th = 0; + root->split_axis = 0; + root->left = Ref(); + root->right = Ref(); +} // resets KDtree + +void AnimationNodeMotionMatch::set_start_index(int32_t si) { + this->start_index = si; +} // set from which point you want to start evaluation + +int32_t AnimationNodeMotionMatch::get_start_index() { + return this->start_index; +} + +void AnimationNodeMotionMatch::calc_root_threshold() { + if (this->point_coordinates.size() == 0) { + print_line("ERROR:Load Points first!"); + } else { + root->calculate_threshold(point_coordinates, dim_len); + } +} // Calculating root threshold TODO : Add options for threshold + +void AnimationNodeMotionMatch::set_min_leaves(int32_t min_l) { + min_leaves = min_l; +} // min_leaves per node + +int32_t AnimationNodeMotionMatch::get_min_leaves() { + return min_leaves; +} + +void AnimationNodeMotionMatch::build_tree() { + if (error == true) { + if (err == LOAD_POINT_ERROR) + print_line("ERROR: Check the input points"); + else if (err == QUERY_POINT_ERROR) + print_line("ERROR: Check your query point"); + else if (err == K_ERROR) + print_line("ERROR: Invalid value for number of neighbors"); + + } else { + print_line("PROCESS : Building KD-Tree.."); + root->leaf_split(point_coordinates, dim_len, start_index, min_leaves); + print_line("KD-Tree built SUCCESSFULLY"); + } +} // Builds the tree with all the given parameters + +float dist_between(PoolRealArray point_coordinates, PoolRealArray p1, + uint32_t index) { + float n = 0; + for (int i = 0; i < p1.size(); i++) { + n += (p1[i] - point_coordinates[p1.size() * index + i]) * + (p1[i] - point_coordinates[p1.size() * index + i]); + } + return sqrt(n); +} + +bool in_array(PoolRealArray points, uint32_t query) { + for (int i = 0; i < points.size(); i++) { + if (points[i] == query) + return true; + } + return false; +} + +PoolRealArray AnimationNodeMotionMatch::KNNSearch(PoolRealArray point, + int32_t k) { + if (error == false && point.size() % dim_len != 0) { + error = true; + err = QUERY_POINT_ERROR; + } else if (error == false && k > point_coordinates.size() / dim_len) { + error = true; + err = K_ERROR; + } + + if (error == true) { + if (err == LOAD_POINT_ERROR) + print_line("ERROR: Check the input points"); + else if (err == QUERY_POINT_ERROR) + print_line("ERROR: Check your query point"); + else if (err == K_ERROR) + print_line("ERROR: Invalid value for number of neighbors"); + + return {}; + + } else { + PoolRealArray Knn = {}; + KDNode *m_node; /*node where the query point can be placed*/ + KDNode *node = root.ptr(); + + while (node->get_indices().size() > min_leaves) { + if (point[node->get_split_axis()] > node->get_th()) { + node = node->get_left(); + } else { + node = node->get_right(); + } + } + m_node = node; + for (int j = 0; j < k; j++) { + uint32_t nn = 0; /*nearest neighbour*/ + float nd = std::numeric_limits::max(); /*nearest distance*/ + for (int i = 0; i < node->get_indices().size(); i++) { + if (j == 0) { + float dist = + dist_between(point_coordinates, point, node->get_indices()[i]); + if (nd > dist || nd == 0) { + nd = dist; + nn = node->get_indices()[i]; + } + } else if (!in_array(Knn, node->get_indices()[i])) { + float dist = + dist_between(point_coordinates, point, node->get_indices()[i]); + if (nd > dist || nd == 0) { + nd = dist; + nn = node->get_indices()[i]; + } + } + } + while (node->get_prev() != NULL) { + node = node->get_prev(); + if ((point[node->get_split_axis()] - node->get_th()) > nd) { + break; + } else { + for (int i = 0; i < node->get_indices().size(); i++) { + if (j == 0) { + float dist = dist_between(point_coordinates, point, + node->get_indices()[i]); + if (nd > dist || nd == 0) { + nd = dist; + nn = node->get_indices()[i]; + } + } else if (!in_array(Knn, node->get_indices()[i])) { + float dist = dist_between(point_coordinates, point, + node->get_indices()[i]); + if (nd > dist || nd == 0) { + nd = dist; + nn = node->get_indices()[i]; + } + } + } + } + } + + Knn.append(nn); + node = m_node; + print_line("Round" + itos(j) + ":" + itos(nn)); + } + return Knn; + } +} /*returns K nearest neighbours in a PoolRealArray*/ + +void AnimationNodeMotionMatch::KDNode::calculate_threshold( + PoolRealArray point_coordinates, int32_t dim_len) { + if (point_coordinates.size() % dim_len != 0) { + print_line("ERROR: Point coordinates array is of wrong size."); + } else { + this->split_th = 0; + for (int i = 0; i < this->point_indices.size(); i++) { + this->split_th += + point_coordinates[(point_indices[i] * dim_len) + split_axis]; + } + this->split_th = this->split_th / this->point_indices.size(); + } +} // threshold calculating function + +bool AnimationNodeMotionMatch::KDNode::are_all_points_same( + PoolRealArray point_coordinates, int32_t dim_len) { + for (int i = 0; i < dim_len; i++) { + for (int j = 0; j < point_indices.size(); j++) { + float p = point_coordinates[(point_indices[j] * dim_len) + i]; + for (int k = 0; k < point_indices.size(); k++) { + if (point_coordinates[(point_indices[k] * dim_len) + i] != p) { + return false; + } + } + } + } + + return true; +} + +void AnimationNodeMotionMatch::KDNode::set_th(float t) { + this->split_th = t; +} + +float AnimationNodeMotionMatch::KDNode::get_th() { + return this->split_th; +} + +void AnimationNodeMotionMatch::KDNode::add_index(uint32_t i) { + point_indices.append(i); +} + +void AnimationNodeMotionMatch::KDNode::clear_indices() { + point_indices.resize(0); +} + +PoolRealArray AnimationNodeMotionMatch::KDNode::get_indices() { + return point_indices; +} + +AnimationNodeMotionMatch::KDNode::KDNode() { + this->point_indices = {}; + this->split_th = 0; + this->split_axis = 0; +} + +void AnimationNodeMotionMatch::KDNode::leaf_split( + PoolRealArray point_coordinates, int32_t dim_len, int32_t dim, + int32_t min_leaves) { + if (point_coordinates.size() % dim_len != 0) { + print_line("ERROR: Point coordinates array is of wrong size."); + } else { + split_axis = dim % dim_len; + calculate_threshold(point_coordinates, dim_len); + + left.instance(); + right.instance(); + + left->prev = this; + right->prev = this; + for (int j = 0; j < this->point_indices.size(); j++) { + if (point_coordinates[(point_indices[j] * dim_len) + split_axis] > + split_th) { + left->point_indices.append(point_indices[j]); + } else { + right->point_indices.append(point_indices[j]); + } + } + + if (left->point_indices.size() > min_leaves && + !are_all_points_same(point_coordinates, dim_len)) { + left->leaf_split(point_coordinates, dim_len, dim + 1, min_leaves); + } + if (right->point_indices.size() > min_leaves && + !are_all_points_same(point_coordinates, dim_len)) { + right->leaf_split(point_coordinates, dim_len, dim + 1, min_leaves); + } + } +} // update tree with given point + +AnimationNodeMotionMatch::KDNode *AnimationNodeMotionMatch::KDNode::get_left() { + return left.ptr(); +} + +AnimationNodeMotionMatch::KDNode * +AnimationNodeMotionMatch::KDNode::get_right() { + return right.ptr(); +} + +float AnimationNodeMotionMatch::process(float p_time, bool p_seek) { + + AnimationPlayer *player = state->player; + + List a_nam; + player->get_animation_list(&a_nam); + // Tracker->Dummy track for modifications + if (!player->has_animation("Tracker")) { + main = a_nam[0]; + Animation *a = player->get_animation(a_nam[0]).ptr(); + + r_index = player->get_animation(a_nam[0]).ptr()->find_track( + state->tree->get_root_motion_track()); + a->track_set_enabled(r_index, false); + + player->add_animation("Tracker", a); + a_nam.clear(); + player->get_animation_list(&a_nam); + } + + if (!timeout && keys->size() != 0 && !editing) { + Vector3 l_v = Vector3(); + l_v = get("velocity"); + future_traj = Predict_traj(l_v, get_parameter(samples)); + float min_cost = std::numeric_limits::max(); + float min_cost_time = 0; + int dup; + + if (first_time) { + dup = -1; + first_time = false; + player->play("Tracker"); + } else { + dup = get_parameter(min); + } + + int p = 0; + print_line(itos(get_instance_id())); + for (p = 0; p < keys->size(); p++) { + if (p != dup) { + float pos_cost = 0.0f; + float traj_cost = 0.0f; + float tot_cost = 0.0f; + + PoolVector::Read read = keys->read(); + + for (int i = 0; i < matching_tracks.size(); i++) { + + Vector s = String(matching_tracks[i]).split(":"); + Vector3 pos = + skeleton->get_bone_global_pose(skeleton->find_bone(s[1])) + .get_origin(); + + for (int po = 0; po < 2; po++) { + pos_cost += (pos[po] - read[p]->bone_data->read()[i][po]) * + (pos[po] - read[p]->bone_data->read()[i][po]); + } + } // calculating pose costs + + for (int t = 0; t < int(get_parameter(samples)) * 2; t++) { + traj_cost += (read[p]->traj[t] - future_traj[t]) * + (read[p]->traj[t] - future_traj[t]); + } // calculating traj costs + + real_t lbd = get_parameter(pvst); // Pose vs Trajectory cost + tot_cost = lbd * pos_cost + (1 - lbd) * traj_cost; + + if (tot_cost < min_cost) { + min_cost = tot_cost; + min_cost_time = keys->read()[p]->time; + set_parameter(min, p); + } // set min + } + } + player->seek(min_cost_time); // play min for every frame + timeout = true; + } + c_time += p_time; + // f_time parameter decides the check rate + if (c_time > real_t(get_parameter(f_time))) { + timeout = false; + c_time = 0; + } + return 0.0; +} + +PoolRealArray AnimationNodeMotionMatch::Predict_traj(Vector3 L_Velocity, + int samples) { + // used exponential decay here + // TODO : Add multiple options to pick for the user + // Can use interpolation functions + PoolRealArray futurepath = {}; + Vector3 c_pos = Vector3(); + + float time = 0; + + for (int i = 0; i < samples; i++) { + c_pos[0] = c_pos[0] + L_Velocity[0] * (1 - Math::exp(-time)); + futurepath.append(c_pos[0]); + c_pos[2] = c_pos[2] + L_Velocity[2] * (1 - Math::exp(-time)); + futurepath.append(c_pos[2]); + + time += delta_time; + } + return futurepath; +} + +void AnimationNodeMotionMatch::print_array(PoolRealArray ar) { + String s = ""; + for (int k = 0; k < ar.size(); k++) { + s += itos(ar.read()[k]) + ","; + } + print_line(s); +} diff --git a/modules/motionmatch/animation_node_motion_match.h b/modules/motionmatch/animation_node_motion_match.h new file mode 100644 index 000000000000..fb3c1dbeb8ed --- /dev/null +++ b/modules/motionmatch/animation_node_motion_match.h @@ -0,0 +1,153 @@ +#ifndef ANIMATION_NODE_MOTION_MATCH_H +#define ANIMATION_NODE_MOTION_MATCH_H + +#include "frame_model.h" + +#include "core/reference.h" +#include "editor/plugins/animation_tree_editor_plugin.h" +#include "scene/3d/physics_body.h" +#include "scene/animation/animation_tree.h" +#include + +class AnimationNodeMotionMatch : public AnimationRootNode { + GDCLASS(AnimationNodeMotionMatch, AnimationRootNode) + + Vector matching_tracks; + // parameters + StringName vel; + StringName pos; + StringName min; + StringName pvst; + StringName samples; + StringName f_time; + + StringName main; + // variables used during matching + bool first_time = true; + float c_time = 0; + bool timeout = false; + Vector3 v = Vector3(); + // KDNode Struct + struct KDNode : public Reference { + /*th -> Threshold*/ + // Variables + PoolRealArray point_indices; + Ref left; + Ref right; + float split_th; + int32_t split_axis; + // Methods + bool are_all_points_same(PoolRealArray point_coordinates, int32_t dim_len); + KDNode *prev; + void calculate_threshold(PoolRealArray points, int32_t dim_len); + + void add_index(uint32_t i); + void clear_indices(); + PoolRealArray get_indices(); + + void leaf_split(PoolRealArray point_coordinates, int32_t dim_len, + int32_t dim, int32_t min_leaves); + + KDNode *get_left(); + KDNode *get_right(); + KDNode *get_prev() { return prev; } + + int32_t get_split_axis() { return split_axis; } + + void set_th(float th); + float get_th(); + KDNode(); + }; + + PoolVector *keys = new PoolVector(); + PoolRealArray future_traj; + PoolRealArray point_coordinates; + Ref root; + int dim_len; /*no of dimensions*/ + int32_t start_index; /*Axis relative to which the first split occurs*/ + int32_t min_leaves; /*Minimum leafs in nodes at the end level*/ + bool error = false; + + enum errortype { LOAD_POINT_ERROR, + QUERY_POINT_ERROR, + K_ERROR }; + + Vector3 velocity; + +protected: + static void _bind_methods(); + Variant get_parameter_default_value( + const StringName &p_parameter) const; + +public: + Skeleton *skeleton; + NodePath root_track = NodePath(); + int r_index; + bool done = false; + bool editing = false; + float delta_time; + virtual void get_parameter_list(List *r_list) const; + + float process(float p_time, bool p_seek); + void add_matching_track(const NodePath &p_track_path); + void remove_matching_track(const NodePath &p_track_path); + bool is_matching_track(const NodePath &p_track_path) const; + Vector get_matching_tracks(); + void update_motion_database(AnimationPlayer *p_animation_player); + + errortype err; + void set_start_index(int32_t si); + int32_t get_start_index(); + + void set_min_leaves(int32_t min_l); + int32_t get_min_leaves(); + + void add_coordinates(PoolRealArray point); + void load_coordinates(PoolRealArray points); + PoolRealArray get_coordinates(); + void clear_coordinates(); + + void set_dim_len(int32_t dim_len); + int32_t get_dim_len(); + + void calc_root_threshold(); + + PoolRealArray KNNSearch(PoolRealArray point, int32_t k); + + void build_tree(); + KDNode *get_root(); + void clear_root(); + + PoolVector *get_keys_data() { return keys; } + + void set_keys_data(PoolVector *kys) { keys = kys; } + void clear_keys() { + + while (keys->size() != 0) { + keys->remove(0); + } + c_time = 0; + timeout = false; + } + + PoolRealArray Predict_traj(Vector3 L_Velocity, int samples); + + int get_traj_samples() { return get_parameter(samples); } + void set_traj_samples(int sa) { set_parameter(samples, sa); } + + Vector3 get_velocity() { return velocity; } + + void set_velocity(Vector3 v) { velocity = v; } + + // for trajectory drawing + + int get_key_size() { return keys->size(); } + PoolRealArray get_key_traj(int k_n) { return keys->read()[k_n]->traj; } + PoolRealArray get_future_traj() { return future_traj; } + + void print_array(PoolRealArray ar); + AnimationNodeMotionMatch(); + ~AnimationNodeMotionMatch(); +}; + +#endif // ANIMATION_NODE_MOTION_MATCH_H diff --git a/modules/motionmatch/config.py b/modules/motionmatch/config.py new file mode 100644 index 000000000000..1c8cd12a2dc0 --- /dev/null +++ b/modules/motionmatch/config.py @@ -0,0 +1,5 @@ +def can_build(env, platform): + return True + +def configure(env): + pass diff --git a/modules/motionmatch/frame_model.h b/modules/motionmatch/frame_model.h new file mode 100644 index 000000000000..3eb4af844b13 --- /dev/null +++ b/modules/motionmatch/frame_model.h @@ -0,0 +1,14 @@ +#ifndef FRAME_MODEL_H +#define FRAME_MODEL_H + +#include "scene/main/node.h" + +struct frame_model { + + PoolVector *bone_data = new PoolVector(); + PoolRealArray traj; + float time = 0.0f; + int anim_num = 0; +}; + +#endif diff --git a/modules/motionmatch/register_types.cpp b/modules/motionmatch/register_types.cpp new file mode 100644 index 000000000000..84de827be9eb --- /dev/null +++ b/modules/motionmatch/register_types.cpp @@ -0,0 +1,56 @@ +/*************************************************************************/ +/* register_types.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "register_types.h" + +#include "animation_node_motion_match.h" + +#ifdef TOOLS_ENABLED +#include "animation_motion_match_editor.h" + +static void _editor_init() { + + AnimationNodeMotionMatchEditor *motion_match_editor = + memnew(AnimationNodeMotionMatchEditor); + AnimationTreeEditor::get_singleton()->add_plugin(motion_match_editor); +} + +#endif + +void register_motionmatch_types() { + + ClassDB::register_class(); + +#ifdef TOOLS_ENABLED + EditorNode::add_init_callback(_editor_init); +#endif +} + +void unregister_motionmatch_types() {} diff --git a/modules/motionmatch/register_types.h b/modules/motionmatch/register_types.h new file mode 100644 index 000000000000..853a6cde0434 --- /dev/null +++ b/modules/motionmatch/register_types.h @@ -0,0 +1,32 @@ +/*************************************************************************/ +/* register_types.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +void register_motionmatch_types(); +void unregister_motionmatch_types();