Merge pull request #18699 from AndreaCatania/ik

Inverse kinematic

11 files changed, 955 insertions, 11 deletions

diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp
index 212ee33ffa..15e40bfba1 100644
--- a/editor/editor_node.cpp
+++ b/editor/editor_node.cpp
@@ -109,6 +109,7 @@
 #include "editor/plugins/shader_graph_editor_plugin.h"
 #include "editor/plugins/skeleton_2d_editor_plugin.h"
 #include "editor/plugins/skeleton_editor_plugin.h"
+#include "editor/plugins/skeleton_ik_editor_plugin.h"
 #include "editor/plugins/spatial_editor_plugin.h"
 #include "editor/plugins/sprite_editor_plugin.h"
 #include "editor/plugins/sprite_frames_editor_plugin.h"
@@ -5596,6 +5597,7 @@ EditorNode::EditorNode() {
 	add_editor_plugin(memnew(AudioBusesEditorPlugin(audio_bus_editor)));
 	add_editor_plugin(memnew(AudioBusesEditorPlugin(audio_bus_editor)));
 	add_editor_plugin(memnew(SkeletonEditorPlugin(this)));
+	add_editor_plugin(memnew(SkeletonIKEditorPlugin(this)));
 	add_editor_plugin(memnew(PhysicalBonePlugin(this)));
 
 	// FIXME: Disabled as (according to reduz) users were complaining that it gets in the way
diff --git a/editor/plugins/skeleton_ik_editor_plugin.cpp b/editor/plugins/skeleton_ik_editor_plugin.cpp
new file mode 100644
index 0000000000..2d343d3edd
--- /dev/null
+++ b/editor/plugins/skeleton_ik_editor_plugin.cpp
@@ -0,0 +1,110 @@
+/*************************************************************************/
+/*  skeleton_ik_editor_plugin.cpp                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 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 "skeleton_ik_editor_plugin.h"
+
+#include "scene/animation/skeleton_ik.h"
+
+void SkeletonIKEditorPlugin::_play() {
+
+	if (!skeleton_ik)
+		return;
+
+	if (!skeleton_ik->get_parent_skeleton())
+		return;
+
+	if (play_btn->is_pressed()) {
+
+		initial_bone_poses.resize(skeleton_ik->get_parent_skeleton()->get_bone_count());
+		for (int i = 0; i < skeleton_ik->get_parent_skeleton()->get_bone_count(); ++i) {
+			initial_bone_poses.write[i] = skeleton_ik->get_parent_skeleton()->get_bone_pose(i);
+		}
+
+		skeleton_ik->start();
+	} else {
+		skeleton_ik->stop();
+
+		if (initial_bone_poses.size() != skeleton_ik->get_parent_skeleton()->get_bone_count())
+			return;
+
+		for (int i = 0; i < skeleton_ik->get_parent_skeleton()->get_bone_count(); ++i) {
+			skeleton_ik->get_parent_skeleton()->set_bone_pose(i, initial_bone_poses[i]);
+		}
+	}
+}
+
+void SkeletonIKEditorPlugin::edit(Object *p_object) {
+
+	if (p_object != skeleton_ik) {
+		if (skeleton_ik) {
+			play_btn->set_pressed(false);
+			_play();
+		}
+	}
+
+	SkeletonIK *s = Object::cast_to<SkeletonIK>(p_object);
+	if (!s)
+		return;
+
+	skeleton_ik = s;
+}
+
+bool SkeletonIKEditorPlugin::handles(Object *p_object) const {
+
+	return p_object->is_class("SkeletonIK");
+}
+
+void SkeletonIKEditorPlugin::make_visible(bool p_visible) {
+
+	if (p_visible)
+		play_btn->show();
+	else
+		play_btn->hide();
+}
+
+void SkeletonIKEditorPlugin::_bind_methods() {
+
+	ClassDB::bind_method("_play", &SkeletonIKEditorPlugin::_play);
+}
+
+SkeletonIKEditorPlugin::SkeletonIKEditorPlugin(EditorNode *p_node) {
+
+	editor = p_node;
+	play_btn = memnew(Button);
+	play_btn->set_icon(editor->get_gui_base()->get_icon("Play", "EditorIcons"));
+	play_btn->set_text(TTR("Play IK"));
+	play_btn->set_toggle_mode(true);
+	play_btn->hide();
+	play_btn->connect("pressed", this, "_play");
+	add_control_to_container(CONTAINER_SPATIAL_EDITOR_MENU, play_btn);
+	skeleton_ik = NULL;
+}
+
+SkeletonIKEditorPlugin::~SkeletonIKEditorPlugin() {}
diff --git a/editor/plugins/skeleton_ik_editor_plugin.h b/editor/plugins/skeleton_ik_editor_plugin.h
new file mode 100644
index 0000000000..e645bea39a
--- /dev/null
+++ b/editor/plugins/skeleton_ik_editor_plugin.h
@@ -0,0 +1,65 @@
+/*************************************************************************/
+/*  skeleton_ik_editor_plugin.h                                             */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 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.                */
+/*************************************************************************/
+
+#ifndef SKELETON_IK_EDITOR_PLUGIN_H
+#define SKELETON_IK_EDITOR_PLUGIN_H
+
+#include "editor/editor_node.h"
+#include "editor/editor_plugin.h"
+
+class SkeletonIK;
+
+class SkeletonIKEditorPlugin : public EditorPlugin {
+
+	GDCLASS(SkeletonIKEditorPlugin, EditorPlugin);
+
+	SkeletonIK *skeleton_ik;
+
+	Button *play_btn;
+	EditorNode *editor;
+	Vector<Transform> initial_bone_poses;
+
+	void _play();
+
+protected:
+	static void _bind_methods();
+
+public:
+	virtual String get_name() const { return "SkeletonIK"; }
+	bool has_main_screen() const { return false; }
+	virtual void edit(Object *p_object);
+	virtual bool handles(Object *p_object) const;
+	virtual void make_visible(bool p_visible);
+
+	SkeletonIKEditorPlugin(EditorNode *p_node);
+	~SkeletonIKEditorPlugin();
+};
+
+#endif // SKELETON_IK_EDITOR_PLUGIN_H
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index 4143989671..80bf422c98 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -557,6 +557,7 @@ protected:
 
 private:
 	static Skeleton *find_skeleton_parent(Node *p_parent);
+
 	void _fix_joint_offset();
 	void _reload_joint();
 
diff --git a/scene/3d/skeleton.cpp b/scene/3d/skeleton.cpp
index 4b6b59b2d3..f45cb02211 100644
--- a/scene/3d/skeleton.cpp
+++ b/scene/3d/skeleton.cpp
@@ -600,9 +600,12 @@ PhysicalBone *Skeleton::_get_physical_bone_parent(int p_bone) {
 void Skeleton::_rebuild_physical_bones_cache() {
 	const int b_size = bones.size();
 	for (int i = 0; i < b_size; ++i) {
-		bones.write[i].cache_parent_physical_bone = _get_physical_bone_parent(i);
-		if (bones[i].physical_bone)
-			bones[i].physical_bone->_on_bone_parent_changed();
+		PhysicalBone *parent_pb = _get_physical_bone_parent(i);
+		if (parent_pb != bones[i].physical_bone) {
+			bones.write[i].cache_parent_physical_bone = parent_pb;
+			if (bones[i].physical_bone)
+				bones[i].physical_bone->_on_bone_parent_changed();
+		}
 	}
 }
 
@@ -740,6 +743,8 @@ void Skeleton::_bind_methods() {
 
 #endif // _3D_DISABLED
 
+	ClassDB::bind_method(D_METHOD("set_bone_ignore_animation", "bone", "ignore"), &Skeleton::set_bone_ignore_animation);
+
 	BIND_CONSTANT(NOTIFICATION_UPDATE_SKELETON);
 }
 
diff --git a/scene/3d/skeleton.h b/scene/3d/skeleton.h
index 9672acb57a..a8413ebaf2 100644
--- a/scene/3d/skeleton.h
+++ b/scene/3d/skeleton.h
@@ -39,6 +39,8 @@
 */
 
 #ifndef _3D_DISABLED
+typedef int BoneId;
+
 class PhysicalBone;
 #endif // _3D_DISABLED
 
@@ -96,7 +98,7 @@ class Skeleton : public Spatial {
 	void _make_dirty();
 	bool dirty;
 
-	//bind helpers
+	// bind helpers
 	Array _get_bound_child_nodes_to_bone(int p_bone) const {
 
 		Array bound;
diff --git a/scene/animation/skeleton_ik.cpp b/scene/animation/skeleton_ik.cpp
new file mode 100644
index 0000000000..cda70c97c9
--- /dev/null
+++ b/scene/animation/skeleton_ik.cpp
@@ -0,0 +1,551 @@
+/*************************************************************************/
+/*  skeleton_ik.cpp                                                      */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2018 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.                */
+/*************************************************************************/
+
+/**
+ * @author AndreaCatania
+ */
+
+#include "skeleton_ik.h"
+
+FabrikInverseKinematic::ChainItem *FabrikInverseKinematic::ChainItem::find_child(const BoneId p_bone_id) {
+	for (int i = childs.size() - 1; 0 <= i; --i) {
+		if (p_bone_id == childs[i].bone) {
+			return &childs.write[i];
+		}
+	}
+	return NULL;
+}
+
+FabrikInverseKinematic::ChainItem *FabrikInverseKinematic::ChainItem::add_child(const BoneId p_bone_id) {
+	const int infant_child_id = childs.size();
+	childs.resize(infant_child_id + 1);
+	childs.write[infant_child_id].bone = p_bone_id;
+	childs.write[infant_child_id].parent_item = this;
+	return &childs.write[infant_child_id];
+}
+
+/// Build a chain that starts from the root to tip
+void FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain) {
+
+	ERR_FAIL_COND(-1 == p_task->root_bone);
+
+	Chain &chain(p_task->chain);
+
+	chain.tips.resize(p_task->end_effectors.size());
+	chain.chain_root.bone = p_task->root_bone;
+	chain.chain_root.initial_transform = p_task->skeleton->get_bone_global_pose(chain.chain_root.bone);
+	chain.chain_root.current_pos = chain.chain_root.initial_transform.origin;
+	chain.chain_root.pb = p_task->skeleton->get_physical_bone(chain.chain_root.bone);
+	chain.middle_chain_item = NULL;
+
+	// Holds all IDs that are composing a single chain in reverse order
+	Vector<BoneId> chain_ids;
+	// This is used to know the chain size
+	int sub_chain_size;
+	// Resize only one time in order to fit all joints for performance reason
+	chain_ids.resize(p_task->skeleton->get_bone_count());
+
+	for (int x = p_task->end_effectors.size() - 1; 0 <= x; --x) {
+
+		const EndEffector *ee(&p_task->end_effectors[x]);
+		ERR_FAIL_COND(p_task->root_bone >= ee->tip_bone);
+		ERR_FAIL_INDEX(ee->tip_bone, p_task->skeleton->get_bone_count());
+
+		sub_chain_size = 0;
+		// Picks all IDs that composing a single chain in reverse order (except the root)
+		BoneId chain_sub_tip(ee->tip_bone);
+		while (chain_sub_tip > p_task->root_bone) {
+
+			chain_ids.write[sub_chain_size++] = chain_sub_tip;
+			chain_sub_tip = p_task->skeleton->get_bone_parent(chain_sub_tip);
+		}
+
+		BoneId middle_chain_item_id = (((float)sub_chain_size) * 0.5);
+
+		// Build chain by reading chain ids in reverse order
+		// For each chain item id will be created a ChainItem if doesn't exists
+		ChainItem *sub_chain(&chain.chain_root);
+		for (int i = sub_chain_size - 1; 0 <= i; --i) {
+
+			ChainItem *child_ci(sub_chain->find_child(chain_ids[i]));
+			if (!child_ci) {
+
+				child_ci = sub_chain->add_child(chain_ids[i]);
+
+				child_ci->pb = p_task->skeleton->get_physical_bone(child_ci->bone);
+
+				child_ci->initial_transform = p_task->skeleton->get_bone_global_pose(child_ci->bone);
+				child_ci->current_pos = child_ci->initial_transform.origin;
+
+				if (child_ci->parent_item) {
+					child_ci->length = (child_ci->current_pos - child_ci->parent_item->current_pos).length();
+				}
+			}
+
+			sub_chain = child_ci;
+
+			if (middle_chain_item_id == i) {
+				chain.middle_chain_item = child_ci;
+			}
+		}
+
+		if (!middle_chain_item_id)
+			chain.middle_chain_item = NULL;
+
+		// Initialize current tip
+		chain.tips.write[x].chain_item = sub_chain;
+		chain.tips.write[x].end_effector = ee;
+
+		if (p_force_simple_chain) {
+			// NOTE:
+			//	This is an "hack" that force to create only one tip per chain since the solver of multi tip (end effector)
+			//	is not yet created.
+			//	Remove this code when this is done
+			break;
+		}
+	}
+}
+
+void FabrikInverseKinematic::update_chain(const Skeleton *p_sk, ChainItem *p_chain_item) {
+
+	if (!p_chain_item)
+		return;
+
+	p_chain_item->initial_transform = p_sk->get_bone_global_pose(p_chain_item->bone);
+	p_chain_item->current_pos = p_chain_item->initial_transform.origin;
+
+	for (int i = p_chain_item->childs.size() - 1; 0 <= i; --i) {
+		update_chain(p_sk, &p_chain_item->childs.write[i]);
+	}
+}
+
+void FabrikInverseKinematic::solve_simple(Task *p_task, bool p_solve_magnet) {
+
+	real_t distance_to_goal(1e4);
+	real_t previous_distance_to_goal(0);
+	int can_solve(p_task->max_iterations);
+	while (distance_to_goal > p_task->min_distance && Math::abs(previous_distance_to_goal - distance_to_goal) > 0.005 && can_solve) {
+		previous_distance_to_goal = distance_to_goal;
+		--can_solve;
+
+		solve_simple_backwards(p_task->chain, p_solve_magnet);
+		solve_simple_forwards(p_task->chain, p_solve_magnet);
+
+		distance_to_goal = (p_task->chain.tips[0].chain_item->current_pos - p_task->chain.tips[0].end_effector->goal_transform.origin).length();
+	}
+}
+
+void FabrikInverseKinematic::solve_simple_backwards(Chain &r_chain, bool p_solve_magnet) {
+
+	if (p_solve_magnet && !r_chain.middle_chain_item) {
+		return;
+	}
+
+	Vector3 goal;
+	ChainItem *sub_chain_tip;
+	if (p_solve_magnet) {
+		goal = r_chain.magnet_position;
+		sub_chain_tip = r_chain.middle_chain_item;
+	} else {
+		goal = r_chain.tips[0].end_effector->goal_transform.origin;
+		sub_chain_tip = r_chain.tips[0].chain_item;
+	}
+
+	while (sub_chain_tip) {
+		sub_chain_tip->current_pos = goal;
+
+		if (sub_chain_tip->parent_item) {
+			// Not yet in the chain root
+			// So calculate next goal location
+
+			const Vector3 look_parent((sub_chain_tip->parent_item->current_pos - sub_chain_tip->current_pos).normalized());
+			goal = sub_chain_tip->current_pos + (look_parent * sub_chain_tip->length);
+
+			// [TODO] Constraints goes here
+		}
+
+		sub_chain_tip = sub_chain_tip->parent_item;
+	}
+}
+
+void FabrikInverseKinematic::solve_simple_forwards(Chain &r_chain, bool p_solve_magnet) {
+
+	if (p_solve_magnet && !r_chain.middle_chain_item) {
+		return;
+	}
+
+	ChainItem *sub_chain_root(&r_chain.chain_root);
+	Vector3 origin(r_chain.chain_root.initial_transform.origin);
+
+	while (sub_chain_root) { // Reach the tip
+		sub_chain_root->current_pos = origin;
+
+		if (!sub_chain_root->childs.empty()) {
+
+			ChainItem &child(sub_chain_root->childs.write[0]);
+
+			// Is not tip
+			// So calculate next origin location
+
+			// Look child
+			sub_chain_root->current_ori = (child.current_pos - sub_chain_root->current_pos).normalized();
+			origin = sub_chain_root->current_pos + (sub_chain_root->current_ori * child.length);
+
+			// [TODO] Constraints goes here
+
+			if (p_solve_magnet && sub_chain_root == r_chain.middle_chain_item) {
+				// In case of magnet solving this is the tip
+				sub_chain_root = NULL;
+			} else {
+				sub_chain_root = &child;
+			}
+		} else {
+
+			// Is tip
+			sub_chain_root = NULL;
+		}
+	}
+}
+
+FabrikInverseKinematic::Task *FabrikInverseKinematic::create_simple_task(Skeleton *p_sk, BoneId root_bone, BoneId tip_bone, const Transform &goal_transform) {
+
+	FabrikInverseKinematic::EndEffector ee;
+	ee.tip_bone = tip_bone;
+
+	Task *task(memnew(Task));
+	task->skeleton = p_sk;
+	task->root_bone = root_bone;
+	task->end_effectors.push_back(ee);
+	task->goal_global_transform = goal_transform;
+
+	build_chain(task);
+
+	return task;
+}
+
+void FabrikInverseKinematic::free_task(Task *p_task) {
+	if (p_task)
+		memdelete(p_task);
+}
+
+void FabrikInverseKinematic::set_goal(Task *p_task, const Transform &p_goal) {
+	p_task->goal_global_transform = p_goal;
+}
+
+void FabrikInverseKinematic::make_goal(Task *p_task, const Transform &p_inverse_transf, real_t blending_delta) {
+
+	if (blending_delta >= 0.99f) {
+		// Update the end_effector (local transform) without blending
+		p_task->end_effectors.write[0].goal_transform = p_inverse_transf * p_task->goal_global_transform;
+	} else {
+
+		// End effector in local transform
+		const Transform end_effector_pose(p_task->skeleton->get_bone_global_pose(p_task->end_effectors.write[0].tip_bone));
+
+		// Update the end_effector (local transform) by blending with current pose
+		p_task->end_effectors.write[0].goal_transform = end_effector_pose.interpolate_with(p_inverse_transf * p_task->goal_global_transform, blending_delta);
+	}
+}
+
+void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool p_use_magnet, const Vector3 &p_magnet_position) {
+
+	if (blending_delta <= 0.01f) {
+		return; // Skip solving
+	}
+
+	make_goal(p_task, p_task->skeleton->get_global_transform().affine_inverse().scaled(p_task->skeleton->get_global_transform().get_basis().get_scale()), blending_delta);
+
+	update_chain(p_task->skeleton, &p_task->chain.chain_root);
+
+	if (p_use_magnet && p_task->chain.middle_chain_item) {
+		p_task->chain.magnet_position = p_task->chain.middle_chain_item->initial_transform.origin.linear_interpolate(p_magnet_position, blending_delta);
+		solve_simple(p_task, true);
+	}
+	solve_simple(p_task, false);
+
+	// Assign new bone position.
+	ChainItem *ci(&p_task->chain.chain_root);
+	while (ci) {
+		Transform new_bone_pose(ci->initial_transform);
+		new_bone_pose.origin = ci->current_pos;
+
+		if (!ci->childs.empty()) {
+
+			/// Rotate basis
+			const Vector3 initial_ori((ci->childs[0].initial_transform.origin - ci->initial_transform.origin).normalized());
+			const Vector3 rot_axis(initial_ori.cross(ci->current_ori).normalized());
+
+			if (rot_axis[0] != 0 && rot_axis[1] != 0 && rot_axis[2] != 0) {
+				const real_t rot_angle(Math::acos(CLAMP(initial_ori.dot(ci->current_ori), -1, 1)));
+				new_bone_pose.basis.rotate(rot_axis, rot_angle);
+			}
+		} else {
+			// Set target orientation to tip
+			new_bone_pose.basis = p_task->chain.tips[0].end_effector->goal_transform.basis;
+		}
+
+		p_task->skeleton->set_bone_global_pose(ci->bone, new_bone_pose);
+
+		if (!ci->childs.empty())
+			ci = &ci->childs.write[0];
+		else
+			ci = NULL;
+	}
+}
+
+void SkeletonIK::_validate_property(PropertyInfo &property) const {
+
+	if (property.name == "root_bone" || property.name == "tip_bone") {
+
+		if (skeleton) {
+
+			String names;
+			for (int i = 0; i < skeleton->get_bone_count(); i++) {
+				if (i > 0)
+					names += ",";
+				names += skeleton->get_bone_name(i);
+			}
+
+			property.hint = PROPERTY_HINT_ENUM;
+			property.hint_string = names;
+		} else {
+
+			property.hint = PROPERTY_HINT_NONE;
+			property.hint_string = "";
+		}
+	}
+}
+
+void SkeletonIK::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_root_bone", "root_bone"), &SkeletonIK::set_root_bone);
+	ClassDB::bind_method(D_METHOD("get_root_bone"), &SkeletonIK::get_root_bone);
+
+	ClassDB::bind_method(D_METHOD("set_tip_bone", "tip_bone"), &SkeletonIK::set_tip_bone);
+	ClassDB::bind_method(D_METHOD("get_tip_bone"), &SkeletonIK::get_tip_bone);
+
+	ClassDB::bind_method(D_METHOD("set_interpolation", "interpolation"), &SkeletonIK::set_interpolation);
+	ClassDB::bind_method(D_METHOD("get_interpolation"), &SkeletonIK::get_interpolation);
+
+	ClassDB::bind_method(D_METHOD("set_target_transform", "target"), &SkeletonIK::set_target_transform);
+	ClassDB::bind_method(D_METHOD("get_target_transform"), &SkeletonIK::get_target_transform);
+
+	ClassDB::bind_method(D_METHOD("set_target_node", "node"), &SkeletonIK::set_target_node);
+	ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonIK::get_target_node);
+
+	ClassDB::bind_method(D_METHOD("set_use_magnet", "use"), &SkeletonIK::set_use_magnet);
+	ClassDB::bind_method(D_METHOD("is_using_magnet"), &SkeletonIK::is_using_magnet);
+
+	ClassDB::bind_method(D_METHOD("set_magnet_position", "local_position"), &SkeletonIK::set_magnet_position);
+	ClassDB::bind_method(D_METHOD("get_magnet_position"), &SkeletonIK::get_magnet_position);
+
+	ClassDB::bind_method(D_METHOD("get_parent_skeleton"), &SkeletonIK::get_parent_skeleton);
+	ClassDB::bind_method(D_METHOD("is_running"), &SkeletonIK::is_running);
+
+	ClassDB::bind_method(D_METHOD("set_min_distance", "min_distance"), &SkeletonIK::set_min_distance);
+	ClassDB::bind_method(D_METHOD("get_min_distance"), &SkeletonIK::get_min_distance);
+
+	ClassDB::bind_method(D_METHOD("set_max_iterations", "iterations"), &SkeletonIK::set_max_iterations);
+	ClassDB::bind_method(D_METHOD("get_max_iterations"), &SkeletonIK::get_max_iterations);
+
+	ClassDB::bind_method(D_METHOD("start", "one_time"), &SkeletonIK::start, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("stop"), &SkeletonIK::stop);
+
+	ADD_PROPERTY(PropertyInfo(Variant::STRING, "root_bone"), "set_root_bone", "get_root_bone");
+	ADD_PROPERTY(PropertyInfo(Variant::STRING, "tip_bone"), "set_tip_bone", "get_tip_bone");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "interpolation", PROPERTY_HINT_RANGE, "0,1,0.001"), "set_interpolation", "get_interpolation");
+	ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM, "target"), "set_target", "get_target");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_magnet"), "set_use_magnet", "is_using_magnet");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "magnet"), "set_magnet_position", "get_magnet_position");
+	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_node"), "set_target_node", "get_target_node");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "min_distance"), "set_min_distance", "get_min_distance");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "max_iterations"), "set_max_iterations", "get_max_iterations");
+}
+
+void SkeletonIK::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			skeleton = Object::cast_to<Skeleton>(get_parent());
+			reload_chain();
+		} break;
+		case NOTIFICATION_INTERNAL_PROCESS: {
+
+			if (target_node_override)
+				reload_goal();
+
+			_solve_chain();
+
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+			reload_chain();
+		} break;
+	}
+}
+
+SkeletonIK::SkeletonIK() :
+		Node(),
+		interpolation(1),
+		skeleton(NULL),
+		target_node_override(NULL),
+		use_magnet(false),
+		min_distance(0.01),
+		max_iterations(10),
+		task(NULL) {
+
+	set_process_priority(1);
+}
+
+SkeletonIK::~SkeletonIK() {
+	FabrikInverseKinematic::free_task(task);
+	task = NULL;
+}
+
+void SkeletonIK::set_root_bone(const StringName &p_root_bone) {
+	root_bone = p_root_bone;
+	reload_chain();
+}
+
+StringName SkeletonIK::get_root_bone() const {
+	return root_bone;
+}
+
+void SkeletonIK::set_tip_bone(const StringName &p_tip_bone) {
+	tip_bone = p_tip_bone;
+	reload_chain();
+}
+
+StringName SkeletonIK::get_tip_bone() const {
+	return tip_bone;
+}
+
+void SkeletonIK::set_interpolation(real_t p_interpolation) {
+	interpolation = p_interpolation;
+}
+
+real_t SkeletonIK::get_interpolation() const {
+	return interpolation;
+}
+
+void SkeletonIK::set_target_transform(const Transform &p_target) {
+	target = p_target;
+	reload_goal();
+}
+
+const Transform &SkeletonIK::get_target_transform() const {
+	return target;
+}
+
+void SkeletonIK::set_target_node(const NodePath &p_node) {
+	target_node_path_override = p_node;
+	target_node_override = NULL;
+	reload_goal();
+}
+
+NodePath SkeletonIK::get_target_node() {
+	return target_node_path_override;
+}
+
+void SkeletonIK::set_use_magnet(bool p_use) {
+	use_magnet = p_use;
+}
+
+bool SkeletonIK::is_using_magnet() const {
+	return use_magnet;
+}
+
+void SkeletonIK::set_magnet_position(const Vector3 &p_local_position) {
+	magnet_position = p_local_position;
+}
+
+const Vector3 &SkeletonIK::get_magnet_position() const {
+	return magnet_position;
+}
+
+void SkeletonIK::set_min_distance(real_t p_min_distance) {
+	min_distance = p_min_distance;
+}
+
+void SkeletonIK::set_max_iterations(int p_iterations) {
+	max_iterations = p_iterations;
+}
+
+bool SkeletonIK::is_running() {
+	return is_processing_internal();
+}
+
+void SkeletonIK::start(bool p_one_time) {
+	if (p_one_time) {
+		set_process_internal(false);
+		_solve_chain();
+	} else {
+		set_process_internal(true);
+	}
+}
+
+void SkeletonIK::stop() {
+	set_process_internal(false);
+}
+
+Transform SkeletonIK::_get_target_transform() {
+
+	if (!target_node_override && !target_node_path_override.is_empty())
+		target_node_override = Object::cast_to<Spatial>(get_node(target_node_path_override));
+
+	if (target_node_override)
+		return target_node_override->get_global_transform();
+	else
+		return target;
+}
+
+void SkeletonIK::reload_chain() {
+
+	FabrikInverseKinematic::free_task(task);
+	task = NULL;
+
+	if (!skeleton)
+		return;
+
+	task = FabrikInverseKinematic::create_simple_task(skeleton, skeleton->find_bone(root_bone), skeleton->find_bone(tip_bone), _get_target_transform());
+	task->max_iterations = max_iterations;
+	task->min_distance = min_distance;
+}
+
+void SkeletonIK::reload_goal() {
+	if (!task)
+		return;
+
+	FabrikInverseKinematic::set_goal(task, _get_target_transform());
+}
+
+void SkeletonIK::_solve_chain() {
+	if (!task)
+		return;
+	FabrikInverseKinematic::solve(task, interpolation, use_magnet, magnet_position);
+}
diff --git a/scene/animation/skeleton_ik.h b/scene/animation/skeleton_ik.h
new file mode 100644
index 0000000000..366c599c01
--- /dev/null
+++ b/scene/animation/skeleton_ik.h
@@ -0,0 +1,212 @@
+/*************************************************************************/
+/*  skeleton_ik.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2018 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.                */
+/*************************************************************************/
+
+#ifndef SKELETON_IK_H
+#define SKELETON_IK_H
+
+/**
+ * @author AndreaCatania
+ */
+
+#include "core/math/transform.h"
+#include "scene/3d/skeleton.h"
+
+class FabrikInverseKinematic {
+
+	struct EndEffector {
+		BoneId tip_bone;
+		Transform goal_transform;
+	};
+
+	struct ChainItem {
+
+		Vector<ChainItem> childs;
+		ChainItem *parent_item;
+
+		// Bone info
+		BoneId bone;
+		PhysicalBone *pb;
+
+		real_t length;
+		/// Positions relative to root bone
+		Transform initial_transform;
+		Vector3 current_pos;
+		// Direction from this bone to child
+		Vector3 current_ori;
+
+		ChainItem() :
+				parent_item(NULL),
+				bone(-1),
+				pb(NULL),
+				length(0) {}
+
+		ChainItem *find_child(const BoneId p_bone_id);
+		ChainItem *add_child(const BoneId p_bone_id);
+	};
+
+	struct ChainTip {
+		ChainItem *chain_item;
+		const EndEffector *end_effector;
+
+		ChainTip() :
+				chain_item(NULL),
+				end_effector(NULL) {}
+
+		ChainTip(ChainItem *p_chain_item, const EndEffector *p_end_effector) :
+				chain_item(p_chain_item),
+				end_effector(p_end_effector) {}
+
+		ChainTip(const ChainTip &p_other_ct) :
+				chain_item(p_other_ct.chain_item),
+				end_effector(p_other_ct.end_effector) {}
+	};
+
+	struct Chain {
+		ChainItem chain_root;
+		ChainItem *middle_chain_item;
+		Vector<ChainTip> tips;
+		Vector3 magnet_position;
+	};
+
+public:
+	struct Task : public RID_Data {
+		RID self;
+		Skeleton *skeleton;
+
+		Chain chain;
+
+		// Settings
+		real_t min_distance;
+		int max_iterations;
+
+		// Bone data
+		BoneId root_bone;
+		Vector<EndEffector> end_effectors;
+
+		Transform goal_global_transform;
+
+		Task() :
+				skeleton(NULL),
+				min_distance(0.01),
+				max_iterations(10),
+				root_bone(-1) {}
+	};
+
+private:
+	/// Init a chain that starts from the root to tip
+	static void build_chain(Task *p_task, bool p_force_simple_chain = true);
+
+	static void update_chain(const Skeleton *p_sk, ChainItem *p_chain_item);
+
+	static void solve_simple(Task *p_task, bool p_solve_magnet);
+	/// Special solvers that solve only chains with one end effector
+	static void solve_simple_backwards(Chain &r_chain, bool p_solve_magnet);
+	static void solve_simple_forwards(Chain &r_chain, bool p_solve_magnet);
+
+public:
+	static Task *create_simple_task(Skeleton *p_sk, BoneId root_bone, BoneId tip_bone, const Transform &goal_transform);
+	static void free_task(Task *p_task);
+	// The goal of chain should be always in local space
+	static void set_goal(Task *p_task, const Transform &p_goal);
+	static void make_goal(Task *p_task, const Transform &p_inverse_transf, real_t blending_delta);
+	static void solve(Task *p_task, real_t blending_delta, bool p_use_magnet, const Vector3 &p_magnet_position);
+};
+
+class SkeletonIK : public Node {
+	GDCLASS(SkeletonIK, Node);
+
+	StringName root_bone;
+	StringName tip_bone;
+	real_t interpolation;
+	Transform target;
+	NodePath target_node_path_override;
+	bool use_magnet;
+	Vector3 magnet_position;
+
+	real_t min_distance;
+	int max_iterations;
+
+	Skeleton *skeleton;
+	Spatial *target_node_override;
+	FabrikInverseKinematic::Task *task;
+
+protected:
+	virtual void
+	_validate_property(PropertyInfo &property) const;
+
+	static void _bind_methods();
+	virtual void _notification(int p_notification);
+
+public:
+	SkeletonIK();
+	virtual ~SkeletonIK();
+
+	void set_root_bone(const StringName &p_root_bone);
+	StringName get_root_bone() const;
+
+	void set_tip_bone(const StringName &p_tip_bone);
+	StringName get_tip_bone() const;
+
+	void set_interpolation(real_t p_interpolation);
+	real_t get_interpolation() const;
+
+	void set_target_transform(const Transform &p_target);
+	const Transform &get_target_transform() const;
+
+	void set_target_node(const NodePath &p_node);
+	NodePath get_target_node();
+
+	void set_use_magnet(bool p_use);
+	bool is_using_magnet() const;
+
+	void set_magnet_position(const Vector3 &p_constraint);
+	const Vector3 &get_magnet_position() const;
+
+	void set_min_distance(real_t p_min_distance);
+	real_t get_min_distance() const { return min_distance; }
+
+	void set_max_iterations(int p_iterations);
+	int get_max_iterations() const { return max_iterations; }
+
+	Skeleton *get_parent_skeleton() const { return skeleton; }
+
+	bool is_running();
+
+	void start(bool p_one_time = false);
+	void stop();
+
+private:
+	Transform _get_target_transform();
+	void reload_chain();
+	void reload_goal();
+	void _solve_chain();
+};
+
+#endif // SKELETON_IK_H
diff --git a/scene/main/node.cpp b/scene/main/node.cpp
index 68de7b3a0a..6144240328 100644
--- a/scene/main/node.cpp
+++ b/scene/main/node.cpp
@@ -1415,11 +1415,6 @@ bool Node::is_greater_than(const Node *p_node) const {
 	return res;
 }
 
-bool Node::has_priority_higher_than(const Node *p_node) const {
-	ERR_FAIL_NULL_V(p_node, false);
-	return data.process_priority > p_node->data.process_priority;
-}
-
 void Node::get_owned_by(Node *p_by, List<Node *> *p_owned) {
 
 	if (data.owner == p_by)
diff --git a/scene/main/node.h b/scene/main/node.h
index 9ee0340678..f3422618ce 100644
--- a/scene/main/node.h
+++ b/scene/main/node.h
@@ -72,7 +72,7 @@ public:
 
 	struct ComparatorWithPriority {
 
-		bool operator()(const Node *p_a, const Node *p_b) const { return p_b->has_priority_higher_than(p_a) || p_b->is_greater_than(p_a); }
+		bool operator()(const Node *p_a, const Node *p_b) const { return p_b->data.process_priority == p_a->data.process_priority ? p_b->is_greater_than(p_a) : p_b->data.process_priority > p_a->data.process_priority; }
 	};
 
 private:
@@ -265,7 +265,6 @@ public:
 
 	bool is_a_parent_of(const Node *p_node) const;
 	bool is_greater_than(const Node *p_node) const;
-	bool has_priority_higher_than(const Node *p_node) const;
 
 	NodePath get_path() const;
 	NodePath get_path_to(const Node *p_node) const;
diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp
index a4fd35304a..ffac166453 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -204,6 +204,7 @@
 #include "scene/3d/sprite_3d.h"
 #include "scene/3d/vehicle_body.h"
 #include "scene/3d/visibility_notifier.h"
+#include "scene/animation/skeleton_ik.h"
 #include "scene/resources/environment.h"
 #include "scene/resources/physics_material.h"
 #endif
@@ -361,6 +362,7 @@ void register_scene_types() {
 	ClassDB::register_class<Spatial>();
 	ClassDB::register_virtual_class<SpatialGizmo>();
 	ClassDB::register_class<Skeleton>();
+	ClassDB::register_class<SkeletonIK>();
 	ClassDB::register_class<AnimationPlayer>();
 	ClassDB::register_class<Tween>();