diff --git a/demos/3d/inverse_kinematics/engine.cfg b/demos/3d/inverse_kinematics/engine.cfg new file mode 100644 index 00000000000..e69de29bb2d diff --git a/demos/3d/inverse_kinematics/main.scn b/demos/3d/inverse_kinematics/main.scn new file mode 100644 index 00000000000..918fd09a3ae Binary files /dev/null and b/demos/3d/inverse_kinematics/main.scn differ diff --git a/modules/ik/SCsub b/modules/ik/SCsub new file mode 100644 index 00000000000..211a043468e --- /dev/null +++ b/modules/ik/SCsub @@ -0,0 +1,3 @@ +Import('env') + +env.add_source_files(env.modules_sources,"*.cpp") diff --git a/modules/ik/config.py b/modules/ik/config.py new file mode 100644 index 00000000000..f9bd7da08d3 --- /dev/null +++ b/modules/ik/config.py @@ -0,0 +1,11 @@ + + +def can_build(platform): + return True + + +def configure(env): + pass + + + diff --git a/modules/ik/ik.cpp b/modules/ik/ik.cpp new file mode 100644 index 00000000000..2133894eeae --- /dev/null +++ b/modules/ik/ik.cpp @@ -0,0 +1,298 @@ +/*************************************************************************/ +/* ik.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* This file is Copyright (c) 2016 Sergey Lapin */ +/* */ +/* 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 "ik.h" + +bool InverseKinematics::_get(const StringName& p_name,Variant &r_ret) const +{ + + if (String(p_name)=="ik_bone") { + + r_ret=get_bone_name(); + return true; + } + + return false; +} + +bool InverseKinematics::_set(const StringName& p_name, const Variant& p_value) +{ + + if (String(p_name)=="ik_bone") { + + set_bone_name(p_value); + return true; + } + + return false; +} + +void InverseKinematics::_get_property_list( List* p_list ) const +{ + + Skeleton *parent=NULL; + if(get_parent()) + parent=get_parent()->cast_to(); + + if (parent) { + + String names; + for(int i=0;iget_bone_count();i++) { + if(i>0) + names+=","; + names+=parent->get_bone_name(i); + } + + p_list->push_back(PropertyInfo(Variant::STRING,"ik_bone",PROPERTY_HINT_ENUM,names)); + } else { + + p_list->push_back(PropertyInfo(Variant::STRING,"ik_bone")); + + } + +} + +void InverseKinematics::_check_bind() +{ + + if (get_parent() && get_parent()->cast_to()) { + Skeleton *sk = get_parent()->cast_to(); + int idx = sk->find_bone(ik_bone); + if (idx!=-1) { + ik_bone_no = idx; + skel = sk; + bound=true; + } + } +} + +void InverseKinematics::_check_unbind() +{ + + if (bound) { + + if (get_parent() && get_parent()->cast_to()) { + Skeleton *sk = get_parent()->cast_to(); + int idx = sk->find_bone(ik_bone); + if (idx!=-1) { + ik_bone_no = idx; + skel = sk; + } + } + bound=false; + } +} + + +void InverseKinematics::set_bone_name(const String& p_name) +{ + + if (is_inside_tree()) + _check_unbind(); + + ik_bone=p_name; + + if (is_inside_tree()) + _check_bind(); +} + +String InverseKinematics::get_bone_name() const +{ + + return ik_bone; +} + +void InverseKinematics::set_iterations(int itn) +{ + + if (is_inside_tree()) + _check_unbind(); + + iterations=itn; + + if (is_inside_tree()) + _check_bind(); +} + +int InverseKinematics::get_iterations() const +{ + + return iterations; +} + +void InverseKinematics::set_chain_size(int cs) +{ + + if (is_inside_tree()) + _check_unbind(); + + chain_size=cs; + chain.clear(); + int cur_bone = ik_bone_no; + int its = chain_size; + set_process(false); + print_line("wtf clean: " + itos(chain.size()) + "/" + itos(chain_size) + " wtf ik bone: " + itos(ik_bone_no)); + while (its > 0 && cur_bone >= 0) { + print_line("wtf pushing: " + itos(chain.size())); + chain.push_back(cur_bone); + cur_bone = skel->get_bone_parent(cur_bone); + its--; + } + set_process(true); + print_line("wtf size: " + itos(chain.size())); + + if (is_inside_tree()) + _check_bind(); +} + +int InverseKinematics::get_chain_size() const +{ + + return chain_size; +} + +void InverseKinematics::set_precision(float p) +{ + + if (is_inside_tree()) + _check_unbind(); + + precision=p; + + if (is_inside_tree()) + _check_bind(); +} + +float InverseKinematics::get_precision() const +{ + + return precision; +} + + +void InverseKinematics::_notification(int p_what) +{ + + switch(p_what) { + + case NOTIFICATION_ENTER_TREE: { + + _check_bind(); + tail_bone = -1; + if (bound) { + for (int i = 0; i < skel->get_bone_count(); i++) + if (skel->get_bone_parent(i) == ik_bone_no) + tail_bone = i; + int cur_bone = ik_bone_no; + int its = chain_size; + while (its > 0 && cur_bone >= 0) { + chain.push_back(cur_bone); + cur_bone = skel->get_bone_parent(cur_bone); + its--; + } + set_process(true); + } + } break; + case NOTIFICATION_PROCESS: { + float delta = get_process_delta_time(); + Spatial *sksp = skel->cast_to(); + if (!bound) + break; + if (!sksp) + break; + Vector3 to = get_translation(); + for (int hump = 0; hump < iterations; hump++) { + int depth = 0; + float olderr = 1000.0; + float psign = 1.0; + bool reached = false; + + for (List::Element *b = chain.front(); b; b = b->next()) { + int cur_bone = b->get(); + Vector3 d = skel->get_bone_global_pose(tail_bone).origin; + Vector3 rg = to; + float err = d.distance_squared_to(rg); + if (err < precision) { + if (!reached && err < precision) + reached = true; + break; + } else + if (reached) + reached = false; + if (err > olderr) + psign = -psign; + Transform mod = skel->get_bone_global_pose(cur_bone); + Quat q1 = Quat(mod.basis).normalized(); + Transform mod2 = mod.looking_at(to, Vector3(0.0, 1.0, 0.0)); + Quat q2 = Quat(mod2.basis).normalized(); + if (psign < 0.0) + q2 = q2.inverse(); + Quat q = q1.slerp(q2, 0.2 / (1.0 + 500.0 * depth)).normalized(); + Transform fin = Transform(q); + fin.origin = mod.origin; + skel->set_bone_global_pose(cur_bone, fin); + depth++; + } + if (reached) + break; + + } + + } break; + case NOTIFICATION_EXIT_TREE: { + set_process(false); + + _check_unbind(); + } break; + } +} +void InverseKinematics::_bind_methods() { + ObjectTypeDB::bind_method(_MD("set_bone_name","ik_bone"),&InverseKinematics::set_bone_name); + ObjectTypeDB::bind_method(_MD("get_bone_name"),&InverseKinematics::get_bone_name); + ObjectTypeDB::bind_method(_MD("set_iterations","iterations"),&InverseKinematics::set_iterations); + ObjectTypeDB::bind_method(_MD("get_iterations"),&InverseKinematics::get_iterations); + ObjectTypeDB::bind_method(_MD("set_chain_size","chain_size"),&InverseKinematics::set_chain_size); + ObjectTypeDB::bind_method(_MD("get_chain_size"),&InverseKinematics::get_chain_size); + ObjectTypeDB::bind_method(_MD("set_precision","precision"),&InverseKinematics::set_precision); + ObjectTypeDB::bind_method(_MD("get_precision"),&InverseKinematics::get_precision); + ADD_PROPERTY(PropertyInfo(Variant::INT, "iterations"), _SCS("set_iterations"), _SCS("get_iterations")); + ADD_PROPERTY(PropertyInfo(Variant::INT, "chain_size"), _SCS("set_chain_size"), _SCS("get_chain_size")); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "precision"), _SCS("set_precision"), _SCS("get_precision")); +} + +InverseKinematics::InverseKinematics() +{ + bound=false; + chain_size = 2; + iterations = 100; + precision = 0.001; + +} + diff --git a/modules/ik/ik.h b/modules/ik/ik.h new file mode 100644 index 00000000000..0fa037d02f4 --- /dev/null +++ b/modules/ik/ik.h @@ -0,0 +1,69 @@ +/*************************************************************************/ +/* ik.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* This file is (c) 2016 Sergey Lapin */ +/* */ +/* 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 IK_H +#define IK_H + +#include "scene/3d/skeleton.h" +class InverseKinematics : public Spatial { + OBJ_TYPE(InverseKinematics, Spatial); + bool bound; + String ik_bone; + int ik_bone_no; + int tail_bone; + int chain_size; + Skeleton *skel; + List chain; + void _check_bind(); + void _check_unbind(); + int iterations; + float precision; + +protected: + bool _set(const StringName& p_name, const Variant& p_value); + bool _get(const StringName& p_name,Variant &r_ret) const; + void _get_property_list( List *p_list) const; + + void _notification(int p_what); + static void _bind_methods(); +public: + Skeleton *get_skeleton(); + void set_bone_name(const String& p_name); + String get_bone_name() const; + void set_iterations(int itn); + int get_iterations() const; + void set_chain_size(int cs); + int get_chain_size() const; + void set_precision(float p); + float get_precision() const; + InverseKinematics(); +}; + +#endif + diff --git a/modules/ik/register_types.cpp b/modules/ik/register_types.cpp new file mode 100644 index 00000000000..e7df7f55b29 --- /dev/null +++ b/modules/ik/register_types.cpp @@ -0,0 +1,47 @@ +/*************************************************************************/ +/* register_types.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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" +#ifndef _3D_DISABLED +#include "object_type_db.h" +#include "ik.h" +#endif + +void register_ik_types() { + +#ifndef _3D_DISABLED + ObjectTypeDB::register_type(); +#endif +} + + + +void unregister_ik_types() { + + +} diff --git a/modules/ik/register_types.h b/modules/ik/register_types.h new file mode 100644 index 00000000000..828917ade7e --- /dev/null +++ b/modules/ik/register_types.h @@ -0,0 +1,30 @@ +/*************************************************************************/ +/* register_types.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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_ik_types(); +void unregister_ik_types();