godotengine/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-11-21 14:57:09 +00:00
Code Releases Wiki Activity
Files
ada95cb543f21bb9f39d5fc0d1f55bcdcf0480e4
godot/scene/3d/two_bone_ik_3d.cpp
Silc Lizard (Tokage) Renew bf22eb25e3 Add IKModifier3D
2025-11-04 02:38:48 +09:00

779 lines
31 KiB
C++
Raw Blame History

/**************************************************************************/
/* two_bone_ik_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 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 "two_bone_ik_3d.h"
bool TwoBoneIK3D::_set(const StringName &p_path, const Variant &p_value) {
String path = p_path;
if (path.begins_with("settings/")) {
int which = path.get_slicec('/', 1).to_int();
String what = path.get_slicec('/', 2);
ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
if (what == "target_node") {
set_target_node(which, p_value);
} else if (what == "pole_node") {
set_pole_node(which, p_value);
} else if (what == "root_bone_name") {
set_root_bone_name(which, p_value);
} else if (what == "root_bone") {
set_root_bone(which, p_value);
} else if (what == "middle_bone_name") {
set_middle_bone_name(which, p_value);
} else if (what == "middle_bone") {
set_middle_bone(which, p_value);
} else if (what == "pole_direction") {
set_pole_direction(which, static_cast<SecondaryDirection>((int)p_value));
} else if (what == "pole_direction_vector") {
set_pole_direction_vector(which, p_value);
} else if (what == "end_bone_name") {
set_end_bone_name(which, p_value);
} else if (what == "end_bone") {
String opt = path.get_slicec('/', 3);
if (opt.is_empty()) {
set_end_bone(which, p_value);
} else if (opt == "direction") {
set_end_bone_direction(which, static_cast<BoneDirection>((int)p_value));
} else if (opt == "length") {
set_end_bone_length(which, p_value);
} else {
return false;
}
} else if (what == "use_virtual_end") {
set_use_virtual_end(which, p_value);
} else if (what == "extend_end_bone") {
set_extend_end_bone(which, p_value);
} else {
return false;
}
}
return true;
}
bool TwoBoneIK3D::_get(const StringName &p_path, Variant &r_ret) const {
String path = p_path;
if (path.begins_with("settings/")) {
int which = path.get_slicec('/', 1).to_int();
String what = path.get_slicec('/', 2);
ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
if (what == "target_node") {
r_ret = get_target_node(which);
} else if (what == "pole_node") {
r_ret = get_pole_node(which);
} else if (what == "root_bone_name") {
r_ret = get_root_bone_name(which);
} else if (what == "root_bone") {
r_ret = get_root_bone(which);
} else if (what == "middle_bone_name") {
r_ret = get_middle_bone_name(which);
} else if (what == "middle_bone") {
r_ret = get_middle_bone(which);
} else if (what == "pole_direction") {
r_ret = (int)get_pole_direction(which);
} else if (what == "pole_direction_vector") {
r_ret = get_pole_direction_vector(which);
} else if (what == "end_bone_name") {
r_ret = get_end_bone_name(which);
} else if (what == "end_bone") {
String opt = path.get_slicec('/', 3);
if (opt.is_empty()) {
r_ret = get_end_bone(which);
} else if (opt == "direction") {
r_ret = (int)get_end_bone_direction(which);
} else if (opt == "length") {
r_ret = get_end_bone_length(which);
} else {
return false;
}
} else if (what == "use_virtual_end") {
r_ret = is_using_virtual_end(which);
} else if (what == "extend_end_bone") {
r_ret = is_end_bone_extended(which);
} else {
return false;
}
}
return true;
}
void TwoBoneIK3D::_get_property_list(List<PropertyInfo> *p_list) const {
String enum_hint;
Skeleton3D *skeleton = get_skeleton();
if (skeleton) {
enum_hint = skeleton->get_concatenated_bone_names();
}
LocalVector<PropertyInfo> props;
for (uint32_t i = 0; i < settings.size(); i++) {
String path = "settings/" + itos(i) + "/";
props.push_back(PropertyInfo(Variant::NODE_PATH, path + "target_node"));
props.push_back(PropertyInfo(Variant::NODE_PATH, path + "pole_node"));
props.push_back(PropertyInfo(Variant::STRING, path + "root_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
props.push_back(PropertyInfo(Variant::INT, path + "root_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
props.push_back(PropertyInfo(Variant::STRING, path + "middle_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
props.push_back(PropertyInfo(Variant::INT, path + "middle_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
props.push_back(PropertyInfo(Variant::INT, path + "pole_direction", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_secondary_direction()));
props.push_back(PropertyInfo(Variant::VECTOR3, path + "pole_direction_vector"));
props.push_back(PropertyInfo(Variant::STRING, path + "end_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
props.push_back(PropertyInfo(Variant::INT, path + "end_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
props.push_back(PropertyInfo(Variant::BOOL, path + "use_virtual_end"));
props.push_back(PropertyInfo(Variant::BOOL, path + "extend_end_bone"));
props.push_back(PropertyInfo(Variant::INT, path + "end_bone/direction", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_bone_direction()));
props.push_back(PropertyInfo(Variant::FLOAT, path + "end_bone/length", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"));
}
for (PropertyInfo &p : props) {
_validate_dynamic_prop(p);
p_list->push_back(p);
}
}
void TwoBoneIK3D::_validate_dynamic_prop(PropertyInfo &p_property) const {
PackedStringArray split = p_property.name.split("/");
if (split.size() > 2 && split[0] == "settings") {
int which = split[1].to_int();
bool force_hide = false;
if ((split[2] == "end_bone" || split[2] == "end_bone_name") && split.size() == 3 && is_using_virtual_end(which)) {
p_property.usage = PROPERTY_USAGE_NONE;
}
if (split[2] == "use_virtual_end" && get_middle_bone(which) == -1) {
p_property.usage = PROPERTY_USAGE_NONE;
}
if (split[2] == "extend_end_bone") {
if (is_using_virtual_end(which)) {
p_property.usage = PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_READ_ONLY;
} else if (get_end_bone(which) == -1) {
p_property.usage = PROPERTY_USAGE_NONE;
force_hide = true;
}
}
if (force_hide || (split[2] == "end_bone" && !is_end_bone_extended(which) && split.size() > 3)) {
p_property.usage = PROPERTY_USAGE_NONE;
}
if (split[2] == "pole_direction_vector" && get_pole_direction(which) != SECONDARY_DIRECTION_CUSTOM) {
p_property.usage = PROPERTY_USAGE_NONE;
}
}
}
PackedStringArray TwoBoneIK3D::get_configuration_warnings() const {
PackedStringArray warnings = SkeletonModifier3D::get_configuration_warnings();
for (uint32_t i = 0; i < tb_settings.size(); i++) {
if (tb_settings[i]->target_node.is_empty()) {
warnings.push_back(RTR("Detecting settings with no target set! TwoBoneIK3D must have a target to work."));
break;
}
}
for (uint32_t i = 0; i < tb_settings.size(); i++) {
if (tb_settings[i]->target_node.is_empty()) {
warnings.push_back(RTR("Detecting settings with no pole target set! TwoBoneIK3D must have a pole target to work."));
break;
}
}
return warnings;
}
// Setting.
void TwoBoneIK3D::set_root_bone_name(int p_index, const String &p_bone_name) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->root_bone.name = p_bone_name;
Skeleton3D *sk = get_skeleton();
if (sk) {
set_root_bone(p_index, sk->find_bone(tb_settings[p_index]->root_bone.name));
}
}
String TwoBoneIK3D::get_root_bone_name(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
return tb_settings[p_index]->root_bone.name;
}
void TwoBoneIK3D::set_root_bone(int p_index, int p_bone) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
bool changed = tb_settings[p_index]->root_bone.bone != p_bone;
tb_settings[p_index]->root_bone.bone = p_bone;
Skeleton3D *sk = get_skeleton();
if (sk) {
if (tb_settings[p_index]->root_bone.bone <= -1 || tb_settings[p_index]->root_bone.bone >= sk->get_bone_count()) {
WARN_PRINT("Root bone index out of range!");
tb_settings[p_index]->root_bone.bone = -1;
} else {
tb_settings[p_index]->root_bone.name = sk->get_bone_name(tb_settings[p_index]->root_bone.bone);
}
}
if (changed) {
_update_joints(p_index);
}
}
int TwoBoneIK3D::get_root_bone(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
return tb_settings[p_index]->root_bone.bone;
}
void TwoBoneIK3D::set_middle_bone_name(int p_index, const String &p_bone_name) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->middle_bone.name = p_bone_name;
Skeleton3D *sk = get_skeleton();
if (sk) {
set_middle_bone(p_index, sk->find_bone(tb_settings[p_index]->middle_bone.name));
}
}
String TwoBoneIK3D::get_middle_bone_name(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
return tb_settings[p_index]->middle_bone.name;
}
void TwoBoneIK3D::set_middle_bone(int p_index, int p_bone) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
bool changed = tb_settings[p_index]->middle_bone.bone != p_bone;
tb_settings[p_index]->middle_bone.bone = p_bone;
Skeleton3D *sk = get_skeleton();
if (sk) {
if (tb_settings[p_index]->middle_bone.bone <= -1 || tb_settings[p_index]->middle_bone.bone >= sk->get_bone_count()) {
WARN_PRINT("Middle bone index out of range!");
tb_settings[p_index]->middle_bone.bone = -1;
tb_settings[p_index]->use_virtual_end = false; // To sync inspector.
} else {
tb_settings[p_index]->middle_bone.name = sk->get_bone_name(tb_settings[p_index]->middle_bone.bone);
}
}
if (changed) {
_update_joints(p_index);
}
notify_property_list_changed();
}
int TwoBoneIK3D::get_middle_bone(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
return tb_settings[p_index]->middle_bone.bone;
}
void TwoBoneIK3D::set_end_bone_name(int p_index, const String &p_bone_name) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->end_bone.name = p_bone_name;
Skeleton3D *sk = get_skeleton();
if (sk) {
set_end_bone(p_index, sk->find_bone(tb_settings[p_index]->end_bone.name));
}
}
String TwoBoneIK3D::get_end_bone_name(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
return tb_settings[p_index]->end_bone.name;
}
void TwoBoneIK3D::set_end_bone(int p_index, int p_bone) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
bool changed = tb_settings[p_index]->end_bone.bone != p_bone;
tb_settings[p_index]->end_bone.bone = p_bone;
Skeleton3D *sk = get_skeleton();
if (sk) {
if (tb_settings[p_index]->end_bone.bone <= -1 || tb_settings[p_index]->end_bone.bone >= sk->get_bone_count()) {
WARN_PRINT("End bone index out of range!");
tb_settings[p_index]->end_bone.bone = -1;
} else {
tb_settings[p_index]->end_bone.name = sk->get_bone_name(tb_settings[p_index]->end_bone.bone);
}
}
if (changed) {
_update_joints(p_index);
}
notify_property_list_changed();
}
int TwoBoneIK3D::get_end_bone(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
return tb_settings[p_index]->get_end_bone();
}
void TwoBoneIK3D::set_use_virtual_end(int p_index, bool p_enabled) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
bool changed = tb_settings[p_index]->use_virtual_end != p_enabled;
tb_settings[p_index]->use_virtual_end = p_enabled;
if (p_enabled) {
// To sync inspector.
tb_settings[p_index]->extend_end_bone = true;
}
tb_settings[p_index]->simulation_dirty = true;
if (changed) {
_update_joints(p_index);
}
notify_property_list_changed();
}
bool TwoBoneIK3D::is_using_virtual_end(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
return tb_settings[p_index]->use_virtual_end;
}
void TwoBoneIK3D::set_extend_end_bone(int p_index, bool p_enabled) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->extend_end_bone = p_enabled;
tb_settings[p_index]->simulation_dirty = true;
Skeleton3D *sk = get_skeleton();
if (sk) {
_validate_pole_direction(sk, p_index);
}
notify_property_list_changed();
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
bool TwoBoneIK3D::is_end_bone_extended(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
return tb_settings[p_index]->extend_end_bone;
}
void TwoBoneIK3D::set_end_bone_direction(int p_index, BoneDirection p_bone_direction) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->end_bone_direction = p_bone_direction;
tb_settings[p_index]->simulation_dirty = true;
Skeleton3D *sk = get_skeleton();
if (sk) {
_validate_pole_direction(sk, p_index);
}
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
SkeletonModifier3D::BoneDirection TwoBoneIK3D::get_end_bone_direction(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), BONE_DIRECTION_FROM_PARENT);
return tb_settings[p_index]->end_bone_direction;
}
void TwoBoneIK3D::set_end_bone_length(int p_index, float p_length) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->end_bone_length = p_length;
tb_settings[p_index]->simulation_dirty = true;
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
float TwoBoneIK3D::get_end_bone_length(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), 0);
return tb_settings[p_index]->end_bone_length;
}
void TwoBoneIK3D::set_target_node(int p_index, const NodePath &p_node_path) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->target_node = p_node_path;
update_configuration_warnings();
}
NodePath TwoBoneIK3D::get_target_node(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
return tb_settings[p_index]->target_node;
}
void TwoBoneIK3D::set_pole_node(int p_index, const NodePath &p_node_path) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->pole_node = p_node_path;
update_configuration_warnings();
}
NodePath TwoBoneIK3D::get_pole_node(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
return tb_settings[p_index]->pole_node;
}
void TwoBoneIK3D::set_pole_direction(int p_index, SecondaryDirection p_direction) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
tb_settings[p_index]->pole_direction = p_direction;
tb_settings[p_index]->simulation_dirty = true;
Skeleton3D *sk = get_skeleton();
if (sk) {
_validate_pole_direction(sk, p_index);
}
notify_property_list_changed();
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
SkeletonModifier3D::SecondaryDirection TwoBoneIK3D::get_pole_direction(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), SECONDARY_DIRECTION_NONE);
return tb_settings[p_index]->pole_direction;
}
void TwoBoneIK3D::set_pole_direction_vector(int p_index, const Vector3 &p_vector) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
if (tb_settings[p_index]->pole_direction != SECONDARY_DIRECTION_CUSTOM) {
return;
}
tb_settings[p_index]->pole_direction_vector = p_vector;
tb_settings[p_index]->simulation_dirty = true;
Skeleton3D *sk = get_skeleton();
if (sk) {
_validate_pole_direction(sk, p_index);
}
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
Vector3 TwoBoneIK3D::get_pole_direction_vector(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3());
return tb_settings[p_index]->get_pole_direction_vector();
}
bool TwoBoneIK3D::is_valid(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
return tb_settings[p_index]->root_bone.bone != -1 && tb_settings[p_index]->middle_bone.bone != -1 && tb_settings[p_index]->is_end_valid();
}
void TwoBoneIK3D::_bind_methods() {
// Setting.
ClassDB::bind_method(D_METHOD("set_target_node", "index", "target_node"), &TwoBoneIK3D::set_target_node);
ClassDB::bind_method(D_METHOD("get_target_node", "index"), &TwoBoneIK3D::get_target_node);
ClassDB::bind_method(D_METHOD("set_pole_node", "index", "pole_node"), &TwoBoneIK3D::set_pole_node);
ClassDB::bind_method(D_METHOD("get_pole_node", "index"), &TwoBoneIK3D::get_pole_node);
ClassDB::bind_method(D_METHOD("set_root_bone_name", "index", "bone_name"), &TwoBoneIK3D::set_root_bone_name);
ClassDB::bind_method(D_METHOD("get_root_bone_name", "index"), &TwoBoneIK3D::get_root_bone_name);
ClassDB::bind_method(D_METHOD("set_root_bone", "index", "bone"), &TwoBoneIK3D::set_root_bone);
ClassDB::bind_method(D_METHOD("get_root_bone", "index"), &TwoBoneIK3D::get_root_bone);
ClassDB::bind_method(D_METHOD("set_middle_bone_name", "index", "bone_name"), &TwoBoneIK3D::set_middle_bone_name);
ClassDB::bind_method(D_METHOD("get_middle_bone_name", "index"), &TwoBoneIK3D::get_middle_bone_name);
ClassDB::bind_method(D_METHOD("set_middle_bone", "index", "bone"), &TwoBoneIK3D::set_middle_bone);
ClassDB::bind_method(D_METHOD("get_middle_bone", "index"), &TwoBoneIK3D::get_middle_bone);
ClassDB::bind_method(D_METHOD("set_pole_direction", "index", "direction"), &TwoBoneIK3D::set_pole_direction);
ClassDB::bind_method(D_METHOD("get_pole_direction", "index"), &TwoBoneIK3D::get_pole_direction);
ClassDB::bind_method(D_METHOD("set_pole_direction_vector", "index", "vector"), &TwoBoneIK3D::set_pole_direction_vector);
ClassDB::bind_method(D_METHOD("get_pole_direction_vector", "index"), &TwoBoneIK3D::get_pole_direction_vector);
ClassDB::bind_method(D_METHOD("set_end_bone_name", "index", "bone_name"), &TwoBoneIK3D::set_end_bone_name);
ClassDB::bind_method(D_METHOD("get_end_bone_name", "index"), &TwoBoneIK3D::get_end_bone_name);
ClassDB::bind_method(D_METHOD("set_end_bone", "index", "bone"), &TwoBoneIK3D::set_end_bone);
ClassDB::bind_method(D_METHOD("get_end_bone", "index"), &TwoBoneIK3D::get_end_bone);
ClassDB::bind_method(D_METHOD("set_use_virtual_end", "index", "enabled"), &TwoBoneIK3D::set_use_virtual_end);
ClassDB::bind_method(D_METHOD("is_using_virtual_end", "index"), &TwoBoneIK3D::is_using_virtual_end);
ClassDB::bind_method(D_METHOD("set_extend_end_bone", "index", "enabled"), &TwoBoneIK3D::set_extend_end_bone);
ClassDB::bind_method(D_METHOD("is_end_bone_extended", "index"), &TwoBoneIK3D::is_end_bone_extended);
ClassDB::bind_method(D_METHOD("set_end_bone_direction", "index", "bone_direction"), &TwoBoneIK3D::set_end_bone_direction);
ClassDB::bind_method(D_METHOD("get_end_bone_direction", "index"), &TwoBoneIK3D::get_end_bone_direction);
ClassDB::bind_method(D_METHOD("set_end_bone_length", "index", "length"), &TwoBoneIK3D::set_end_bone_length);
ClassDB::bind_method(D_METHOD("get_end_bone_length", "index"), &TwoBoneIK3D::get_end_bone_length);
ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
}
void TwoBoneIK3D::_validate_bone_names() {
for (uint32_t i = 0; i < settings.size(); i++) {
// Prior bone name.
if (!tb_settings[i]->root_bone.name.is_empty()) {
set_root_bone_name(i, tb_settings[i]->root_bone.name);
} else if (tb_settings[i]->root_bone.bone != -1) {
set_root_bone(i, tb_settings[i]->root_bone.bone);
}
// Prior bone name.
if (!tb_settings[i]->middle_bone.name.is_empty()) {
set_middle_bone_name(i, tb_settings[i]->middle_bone.name);
} else if (tb_settings[i]->middle_bone.bone != -1) {
set_middle_bone(i, tb_settings[i]->middle_bone.bone);
}
// Prior bone name.
if (!tb_settings[i]->end_bone.name.is_empty()) {
set_end_bone_name(i, tb_settings[i]->end_bone.name);
} else if (tb_settings[i]->end_bone.bone != -1) {
set_end_bone(i, tb_settings[i]->end_bone.bone);
}
}
}
void TwoBoneIK3D::_validate_pole_directions(Skeleton3D *p_skeleton) const {
for (uint32_t i = 0; i < settings.size(); i++) {
_validate_pole_direction(p_skeleton, i);
}
}
void TwoBoneIK3D::_validate_pole_direction(Skeleton3D *p_skeleton, int p_index) const {
TwoBoneIK3DSetting *setting = tb_settings[p_index];
SecondaryDirection dir = setting->pole_direction;
if (!is_valid(p_index) || dir == SECONDARY_DIRECTION_NONE) {
return;
}
Vector3 kv = get_pole_direction_vector(p_index).normalized();
Vector3 fwd;
// End bone.
int valid_end_bone = setting->get_end_bone();
Vector3 axis = get_bone_axis(valid_end_bone, setting->end_bone_direction);
Vector3 global_rest_origin;
if (setting->extend_end_bone && setting->end_bone_length > 0 && !axis.is_zero_approx()) {
global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).xform(axis * setting->end_bone_length);
} else {
// Shouldn't be using virtual end.
global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).origin;
}
// Middle bone.
axis = global_rest_origin - p_skeleton->get_bone_global_rest(setting->middle_bone.bone).origin;
if (!axis.is_zero_approx()) {
fwd = p_skeleton->get_bone_global_rest(setting->middle_bone.bone).basis.get_rotation_quaternion().xform_inv(axis).normalized();
} else {
return;
}
if (Math::is_equal_approx(Math::abs(kv.dot(fwd)), 1)) {
WARN_PRINT_ED("Setting: " + itos(p_index) + ": Pole direction and forward vector are colinear. This is not advised as it may cause unwanted rotation.");
}
}
void TwoBoneIK3D::_make_all_joints_dirty() {
for (uint32_t i = 0; i < settings.size(); i++) {
_update_joints(i);
}
}
void TwoBoneIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
TwoBoneIK3DSetting *setting = tb_settings[p_index];
cached_space = p_skeleton->get_global_transform_interpolated();
if (!setting->simulation_dirty) {
return;
}
setting->root_pos = Vector3();
setting->mid_pos = Vector3();
setting->end_pos = Vector3();
if (setting->root_joint_solver_info) {
memdelete(setting->root_joint_solver_info);
setting->root_joint_solver_info = nullptr;
}
if (setting->mid_joint_solver_info) {
memdelete(setting->mid_joint_solver_info);
setting->mid_joint_solver_info = nullptr;
}
if (setting->root_bone.bone == -1 || setting->middle_bone.bone == -1 || !setting->is_end_valid()) {
return;
}
// End bone.
int valid_end_bone = setting->get_end_bone();
Vector3 axis = get_bone_axis(valid_end_bone, setting->end_bone_direction);
Vector3 global_rest_origin;
if (setting->extend_end_bone && setting->end_bone_length > 0 && !axis.is_zero_approx()) {
setting->end_pos = p_skeleton->get_bone_global_pose(valid_end_bone).xform(axis * setting->end_bone_length);
global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).xform(axis * setting->end_bone_length);
} else {
// Shouldn't be using virtual end.
setting->end_pos = p_skeleton->get_bone_global_pose(valid_end_bone).origin;
global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).origin;
}
// Middle bone.
axis = global_rest_origin - p_skeleton->get_bone_global_rest(setting->middle_bone.bone).origin;
global_rest_origin = p_skeleton->get_bone_global_rest(setting->middle_bone.bone).origin;
if (!axis.is_zero_approx()) {
setting->mid_pos = p_skeleton->get_bone_global_pose(setting->middle_bone.bone).origin;
setting->mid_joint_solver_info = memnew(IKModifier3DSolverInfo);
setting->mid_joint_solver_info->forward_vector = p_skeleton->get_bone_global_rest(setting->middle_bone.bone).basis.get_rotation_quaternion().xform_inv(axis).normalized();
setting->mid_joint_solver_info->length = axis.length();
} else {
return;
}
// Root bone.
axis = global_rest_origin - p_skeleton->get_bone_global_rest(setting->root_bone.bone).origin;
global_rest_origin = p_skeleton->get_bone_global_rest(setting->root_bone.bone).origin;
if (!axis.is_zero_approx()) {
setting->root_pos = p_skeleton->get_bone_global_pose(setting->root_bone.bone).origin;
setting->root_joint_solver_info = memnew(IKModifier3DSolverInfo);
setting->root_joint_solver_info->forward_vector = p_skeleton->get_bone_global_rest(setting->root_bone.bone).basis.get_rotation_quaternion().xform_inv(axis).normalized();
setting->root_joint_solver_info->length = axis.length();
} else if (setting->mid_joint_solver_info) {
memdelete(setting->mid_joint_solver_info);
setting->mid_joint_solver_info = nullptr;
return;
}
setting->init_current_joint_rotations(p_skeleton);
double total_length = setting->root_joint_solver_info->length + setting->mid_joint_solver_info->length;
setting->cached_length_sq = total_length * total_length;
setting->simulation_dirty = false;
}
void TwoBoneIK3D::_update_joints(int p_index) {
tb_settings[p_index]->simulation_dirty = true;
#ifdef TOOLS_ENABLED
update_gizmos(); // To clear invalid setting.
#endif // TOOLS_ENABLED
Skeleton3D *sk = get_skeleton();
if (!sk || tb_settings[p_index]->root_bone.bone == -1 || tb_settings[p_index]->middle_bone.bone == -1 || !tb_settings[p_index]->is_end_valid()) {
return;
}
// Validation for middle bone.
int parent_bone = tb_settings[p_index]->root_bone.bone;
int current_bone = tb_settings[p_index]->middle_bone.bone;
bool valid = false;
while (current_bone >= 0) {
if (current_bone == parent_bone) {
valid = true;
break;
}
current_bone = sk->get_bone_parent(current_bone);
}
ERR_FAIL_COND_EDMSG(!valid, "The middle bone must be a child of the root bone.");
// Validation for end bone.
if (!tb_settings[p_index]->use_virtual_end) {
parent_bone = tb_settings[p_index]->middle_bone.bone;
current_bone = tb_settings[p_index]->end_bone.bone;
valid = false;
while (current_bone >= 0) {
if (current_bone == parent_bone) {
valid = true;
break;
}
current_bone = sk->get_bone_parent(current_bone);
}
ERR_FAIL_COND_EDMSG(!valid, "The end bone must be a child of the middle bone.");
}
if (sk) {
_validate_pole_directions(sk);
}
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
void TwoBoneIK3D::_make_simulation_dirty(int p_index) {
TwoBoneIK3DSetting *setting = tb_settings[p_index];
if (!setting) {
return;
}
setting->simulation_dirty = true;
}
void TwoBoneIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
for (uint32_t i = 0; i < settings.size(); i++) {
_init_joints(p_skeleton, i);
Node3D *target = Object::cast_to<Node3D>(get_node_or_null(tb_settings[i]->target_node));
Node3D *pole = Object::cast_to<Node3D>(get_node_or_null(tb_settings[i]->pole_node));
if (!target || !pole || !tb_settings[i]->is_valid()) {
continue; // Abort.
}
Vector3 destination = cached_space.affine_inverse().xform(target->get_global_transform_interpolated().origin);
Vector3 pole_destination = cached_space.affine_inverse().xform(pole->get_global_transform_interpolated().origin);
tb_settings[i]->cache_current_joint_rotations(p_skeleton, pole_destination); // Iterate over first to detect parent (outside of the chain) bone pose changes.
_process_joints(p_delta, p_skeleton, tb_settings[i], destination, pole_destination);
}
}
void TwoBoneIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, TwoBoneIK3DSetting *p_setting, const Vector3 &p_destination, const Vector3 &p_pole_destination) {
// Solve the IK for this iteration.
Vector3 destination = p_destination;
// Make vector from root to destination.
p_setting->root_pos = p_skeleton->get_bone_global_pose(p_setting->root_bone.bone).origin; // New root position.
Vector3 root_to_destination = destination - p_setting->root_pos;
if (root_to_destination.is_zero_approx()) {
return; // Abort.
}
double rd_len_sq = root_to_destination.length_squared();
// Compare the distance to the target with the length of the bones.
if (rd_len_sq >= p_setting->cached_length_sq) {
// Result is straight.
Vector3 rd_nrm = root_to_destination.normalized();
p_setting->mid_pos = p_setting->root_pos + rd_nrm * p_setting->root_joint_solver_info->length;
p_setting->end_pos = p_setting->mid_pos + rd_nrm * p_setting->mid_joint_solver_info->length;
} else {
// Check if the target can be reached by subtracting the lengths of the bones.
// If not, push out target to normal of the root bone sphere.
double sub = p_setting->root_joint_solver_info->length - p_setting->mid_joint_solver_info->length;
if (rd_len_sq < sub * sub) {
Vector3 push_nrm = (destination - p_setting->root_pos).normalized();
destination = p_setting->root_pos + push_nrm * Math::abs(sub);
root_to_destination = destination - p_setting->root_pos;
}
// End is snapped to the target.
p_setting->end_pos = destination;
// Result is bent, determine the mid position to respect the pole target.
// Mid-position should be a point of intersection of two circles.
double l_chain = root_to_destination.length();
Vector3 u = root_to_destination.normalized();
Vector3 pole_vec = get_projected_normal(p_setting->root_pos, p_setting->end_pos, p_pole_destination);
// Circle1: center is the root, radius is the length of the root bone.
double r_root = p_setting->root_joint_solver_info->length;
// Circle2: center is the target, radius is the length of the middle bone.
double r_mid = p_setting->mid_joint_solver_info->length;
double a = (l_chain * l_chain + r_root * r_root - r_mid * r_mid) / (2.0 * l_chain);
double h2 = r_root * r_root - a * a;
if (h2 < 0) {
h2 = 0;
}
double h = Math::sqrt(h2);
Vector3 det_plus = (p_setting->root_pos + u * a) + pole_vec * h;
Vector3 det_minus = (p_setting->root_pos + u * a) - pole_vec * h;
// Pick the intersection that is closest to the pole target.
p_setting->mid_pos = p_pole_destination.distance_squared_to(det_plus) < p_pole_destination.distance_squared_to(det_minus) ? det_plus : det_minus;
}
// Update virtual bone rest/poses.
p_setting->cache_current_vectors(p_skeleton);
p_setting->cache_current_joint_rotations(p_skeleton, p_pole_destination);
// Apply the virtual bone rest/poses to the actual bones.
p_skeleton->set_bone_pose_rotation(p_setting->root_bone.bone, p_setting->root_joint_solver_info->current_lpose);
// Mid joint pose is relative to the root joint pose for the case root-mid or mid-end have more than 1 joints.
p_skeleton->set_bone_pose_rotation(p_setting->middle_bone.bone, get_local_pose_rotation(p_skeleton, p_setting->middle_bone.bone, p_setting->mid_joint_solver_info->current_gpose));
}
TwoBoneIK3D::~TwoBoneIK3D() {
clear_settings();
}
View Git Blame Copy Permalink