InverseKinematics node, basic features
I don't already know how the fuck it works, but it is. A bit slow currently, but hope to improve it soon. The current limitations: 1. No constraints. At all. 2. Used simplest CCD algorithm, I just can't believe in jacobian construction from code. 3. Slow to get to target.
This commit is contained in:
Sergey Lapin
parent
15d1fca061
commit
8d7a94389a
8 changed files with 458 additions and 0 deletions
0
demos/3d/inverse_kinematics/engine.cfg
Normal file
0
demos/3d/inverse_kinematics/engine.cfg
Normal file
BIN
demos/3d/inverse_kinematics/main.scn
Normal file
BIN
demos/3d/inverse_kinematics/main.scn
Normal file
Binary file not shown.
3
modules/ik/SCsub
Normal file
3
modules/ik/SCsub
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
Import('env')
|
||||
|
||||
env.add_source_files(env.modules_sources,"*.cpp")
|
||||
11
modules/ik/config.py
Normal file
11
modules/ik/config.py
Normal file
|
|
@ -0,0 +1,11 @@
|
|||
|
||||
|
||||
def can_build(platform):
|
||||
return True
|
||||
|
||||
|
||||
def configure(env):
|
||||
pass
|
||||
|
||||
|
||||
|
||||
298
modules/ik/ik.cpp
Normal file
298
modules/ik/ik.cpp
Normal file
|
|
@ -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 <slapinid@gmail.com> */
|
||||
/* */
|
||||
/* 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<PropertyInfo>* p_list ) const
|
||||
{
|
||||
|
||||
Skeleton *parent=NULL;
|
||||
if(get_parent())
|
||||
parent=get_parent()->cast_to<Skeleton>();
|
||||
|
||||
if (parent) {
|
||||
|
||||
String names;
|
||||
for(int i=0;i<parent->get_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>()) {
|
||||
Skeleton *sk = get_parent()->cast_to<Skeleton>();
|
||||
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>()) {
|
||||
Skeleton *sk = get_parent()->cast_to<Skeleton>();
|
||||
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<Spatial>();
|
||||
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<int>::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;
|
||||
|
||||
}
|
||||
|
||||
69
modules/ik/ik.h
Normal file
69
modules/ik/ik.h
Normal file
|
|
@ -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 <slapinid@gmail.com> */
|
||||
/* */
|
||||
/* 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<int> 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<PropertyInfo> *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
|
||||
|
||||
47
modules/ik/register_types.cpp
Normal file
47
modules/ik/register_types.cpp
Normal file
|
|
@ -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<InverseKinematics>();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
void unregister_ik_types() {
|
||||
|
||||
|
||||
}
|
||||
30
modules/ik/register_types.h
Normal file
30
modules/ik/register_types.h
Normal file
|
|
@ -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();
|
||||
Loading…
Reference in a new issue