/*************************************************************************/ /* 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::set_speed(float p) { if (is_inside_tree()) _check_unbind(); speed=p; if (is_inside_tree()) _check_bind(); } float InverseKinematics::get_speed() const { return speed; } 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, speed / (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); ObjectTypeDB::bind_method(_MD("set_speed","speed"),&InverseKinematics::set_speed); ObjectTypeDB::bind_method(_MD("get_speed"),&InverseKinematics::get_speed); 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")); ADD_PROPERTY(PropertyInfo(Variant::REAL, "speed"), _SCS("set_speed"), _SCS("get_speed")); } InverseKinematics::InverseKinematics() { bound=false; chain_size = 2; iterations = 100; precision = 0.001; speed = 0.2; }