Stowage/godot
2
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-12-08 06:09:55 +00:00
Code Issues Projects Releases Packages Wiki Activity

Add IKModifier3D

Browse source
This commit is contained in:
Silc Lizard (Tokage) Renew 2025-09-10 09:46:41 +09:00
parent 08705259f2
commit bf22eb25e3
69 changed files with 6066 additions and 179 deletions
Download patch file Download diff file Expand all files Collapse all files

2
scene/3d/aim_modifier_3d.cpp
View file

@ -84,7 +84,7 @@ void AimModifier3D::_get_property_list(List<PropertyInfo> *p_list) const {
String path = "settings/" + itos(i) + "/";
int rotation_usage = is_using_euler(i) ? PROPERTY_USAGE_DEFAULT : PROPERTY_USAGE_NONE;
p_list->push_back(PropertyInfo(Variant::INT, path + "forward_axis", PROPERTY_HINT_ENUM, "+X,-X,+Y,-Y,+Z,-Z"));
p_list->push_back(PropertyInfo(Variant::INT, path + "forward_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_bone_axis()));
p_list->push_back(PropertyInfo(Variant::BOOL, path + "use_euler"));
p_list->push_back(PropertyInfo(Variant::INT, path + "primary_rotation_axis", PROPERTY_HINT_ENUM, "X,Y,Z", rotation_usage));
p_list->push_back(PropertyInfo(Variant::BOOL, path + "use_secondary_rotation", PROPERTY_HINT_NONE, "", rotation_usage));

39
scene/3d/bone_constraint_3d.cpp
View file

@ -270,45 +270,6 @@ void BoneConstraint3D::_process_constraint(int p_index, Skeleton3D *p_skeleton,
//
}
double BoneConstraint3D::symmetrize_angle(double p_angle) {
double angle = Math::fposmod(p_angle, Math::TAU);
return angle > Math::PI ? angle - Math::TAU : angle;
}
double BoneConstraint3D::get_roll_angle(const Quaternion &p_rotation, const Vector3 &p_roll_axis) {
// Ensure roll axis is normalized.
Vector3 roll_axis = p_roll_axis.normalized();
// Project the quaternion rotation onto the roll axis.
// This gives us the component of rotation around that axis.
double dot = p_rotation.x * roll_axis.x +
p_rotation.y * roll_axis.y +
p_rotation.z * roll_axis.z;
// Create a quaternion representing just the roll component.
Quaternion roll_component;
roll_component.x = roll_axis.x * dot;
roll_component.y = roll_axis.y * dot;
roll_component.z = roll_axis.z * dot;
roll_component.w = p_rotation.w;
// Normalize this component.
double length = roll_component.length();
if (length > CMP_EPSILON) {
roll_component = roll_component / length;
} else {
return 0.0;
}
// Extract the angle.
double angle = 2.0 * Math::acos(CLAMP(roll_component.w, -1.0, 1.0));
// Determine the sign.
double direction = (roll_component.x * roll_axis.x + roll_component.y * roll_axis.y + roll_component.z * roll_axis.z > 0) ? 1.0 : -1.0;
return symmetrize_angle(angle * direction);
}
BoneConstraint3D::~BoneConstraint3D() {
clear_settings();
}

3
scene/3d/bone_constraint_3d.h
View file

@ -83,8 +83,5 @@ public:
void clear_settings();
static double symmetrize_angle(double p_angle); // Helper to make angle 0->TAU become -PI->PI.
static double get_roll_angle(const Quaternion &p_rotation, const Vector3 &p_roll_axis);
~BoneConstraint3D();
};

71
scene/3d/ccd_ik_3d.cpp Normal file
View file

@ -0,0 +1,71 @@
/**************************************************************************/
/* ccd_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 "ccd_ik_3d.h"
void CCDIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
int joint_size = (int)p_setting->joints.size();
int chain_size = (int)p_setting->chain.size();
// Backwards.
for (int ancestor = joint_size - 1; ancestor >= 0; ancestor--) {
// Forwards.
for (int i = ancestor; i < joint_size; i++) {
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
int HEAD = i;
int TAIL = i + 1;
Vector3 current_head = p_setting->chain[HEAD];
Vector3 current_effector = p_setting->chain[chain_size - 1];
Vector3 head_to_effector = current_effector - current_head;
Vector3 head_to_destination = p_destination - current_head;
if (Math::is_zero_approx(head_to_destination.length_squared() * head_to_effector.length_squared())) {
continue;
}
Quaternion to_rot = Quaternion(head_to_effector.normalized(), head_to_destination.normalized());
Vector3 to_tail = p_setting->chain[TAIL] - current_head;
p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, current_head + to_rot.xform(to_tail));
if (p_setting->joint_settings[HEAD]->rotation_axis != ROTATION_AXIS_ALL) {
p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, p_setting->chain[HEAD] + p_setting->joint_settings[HEAD]->get_projected_rotation(solver_info->current_grest, p_setting->chain[TAIL] - p_setting->chain[HEAD]));
}
if (p_setting->joint_settings[HEAD]->limitation.is_valid()) {
p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, p_setting->chain[HEAD] + p_setting->joint_settings[HEAD]->get_limited_rotation(solver_info->current_grest, p_setting->chain[TAIL] - p_setting->chain[HEAD], solver_info->forward_vector));
}
}
}
}

40
scene/3d/ccd_ik_3d.h Normal file
View file

@ -0,0 +1,40 @@
/**************************************************************************/
/* ccd_ik_3d.h */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#pragma once
#include "scene/3d/iterate_ik_3d.h"
class CCDIK3D : public IterateIK3D {
GDCLASS(CCDIK3D, IterateIK3D);
protected:
virtual void _solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) override;
};

485
scene/3d/chain_ik_3d.cpp Normal file
View file

@ -0,0 +1,485 @@
/**************************************************************************/
/* chain_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 "chain_ik_3d.h"
bool ChainIK3D::_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 == "root_bone_name") {
set_root_bone_name(which, p_value);
} else if (what == "root_bone") {
set_root_bone(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 == "extend_end_bone") {
set_extend_end_bone(which, p_value);
} else if (what == "joint_count") {
set_joint_count(which, p_value);
} else if (what == "joints") {
int idx = path.get_slicec('/', 3).to_int();
String prop = path.get_slicec('/', 4);
if (prop == "bone_name") {
set_joint_bone_name(which, idx, p_value);
} else if (prop == "bone") {
set_joint_bone(which, idx, p_value);
} else {
return false;
}
} else {
return false;
}
}
return true;
}
bool ChainIK3D::_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 == "root_bone_name") {
r_ret = get_root_bone_name(which);
} else if (what == "root_bone") {
r_ret = get_root_bone(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 == "extend_end_bone") {
r_ret = is_end_bone_extended(which);
} else if (what == "joint_count") {
r_ret = get_joint_count(which);
} else if (what == "joints") {
int idx = path.get_slicec('/', 3).to_int();
String prop = path.get_slicec('/', 4);
if (prop == "bone_name") {
r_ret = get_joint_bone_name(which, idx);
} else if (prop == "bone") {
r_ret = get_joint_bone(which, idx);
} else {
return false;
}
} else {
return false;
}
}
return true;
}
void ChainIK3D::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::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 + "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 + "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"));
props.push_back(PropertyInfo(Variant::INT, path + "joint_count", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_ARRAY, "Joints," + path + "joints/,static,const"));
for (uint32_t j = 0; j < chain_settings[i]->joints.size(); j++) {
String joint_path = path + "joints/" + itos(j) + "/";
props.push_back(PropertyInfo(Variant::STRING, joint_path + "bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint, PROPERTY_USAGE_EDITOR | PROPERTY_USAGE_READ_ONLY));
props.push_back(PropertyInfo(Variant::INT, joint_path + "bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_READ_ONLY));
}
}
for (PropertyInfo &p : props) {
_validate_dynamic_prop(p);
p_list->push_back(p);
}
}
void ChainIK3D::_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();
// Extended end bone option.
bool force_hide = false;
if (split[2] == "extend_end_bone" && 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;
}
}
}
// Setting.
void ChainIK3D::set_root_bone_name(int p_index, const String &p_bone_name) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
chain_settings[p_index]->root_bone.name = p_bone_name;
Skeleton3D *sk = get_skeleton();
if (sk) {
set_root_bone(p_index, sk->find_bone(chain_settings[p_index]->root_bone.name));
}
}
String ChainIK3D::get_root_bone_name(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
return chain_settings[p_index]->root_bone.name;
}
void ChainIK3D::set_root_bone(int p_index, int p_bone) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
bool changed = chain_settings[p_index]->root_bone.bone != p_bone;
chain_settings[p_index]->root_bone.bone = p_bone;
Skeleton3D *sk = get_skeleton();
if (sk) {
if (chain_settings[p_index]->root_bone.bone <= -1 || chain_settings[p_index]->root_bone.bone >= sk->get_bone_count()) {
WARN_PRINT("Root bone index out of range!");
chain_settings[p_index]->root_bone.bone = -1;
} else {
chain_settings[p_index]->root_bone.name = sk->get_bone_name(chain_settings[p_index]->root_bone.bone);
}
}
if (changed) {
_update_joints(p_index);
}
}
int ChainIK3D::get_root_bone(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
return chain_settings[p_index]->root_bone.bone;
}
void ChainIK3D::set_end_bone_name(int p_index, const String &p_bone_name) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
chain_settings[p_index]->end_bone.name = p_bone_name;
Skeleton3D *sk = get_skeleton();
if (sk) {
set_end_bone(p_index, sk->find_bone(chain_settings[p_index]->end_bone.name));
}
}
String ChainIK3D::get_end_bone_name(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
return chain_settings[p_index]->end_bone.name;
}
void ChainIK3D::set_end_bone(int p_index, int p_bone) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
bool changed = chain_settings[p_index]->end_bone.bone != p_bone;
chain_settings[p_index]->end_bone.bone = p_bone;
Skeleton3D *sk = get_skeleton();
if (sk) {
if (chain_settings[p_index]->end_bone.bone <= -1 || chain_settings[p_index]->end_bone.bone >= sk->get_bone_count()) {
WARN_PRINT("End bone index out of range!");
chain_settings[p_index]->end_bone.bone = -1;
} else {
chain_settings[p_index]->end_bone.name = sk->get_bone_name(chain_settings[p_index]->end_bone.bone);
}
}
if (changed) {
_update_joints(p_index);
}
notify_property_list_changed();
}
int ChainIK3D::get_end_bone(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
return chain_settings[p_index]->end_bone.bone;
}
void ChainIK3D::set_extend_end_bone(int p_index, bool p_enabled) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
chain_settings[p_index]->extend_end_bone = p_enabled;
_make_simulation_dirty(p_index);
Skeleton3D *sk = get_skeleton();
if (sk && !chain_settings[p_index]->joints.is_empty()) {
_validate_axis(sk, p_index, chain_settings[p_index]->joints.size() - 1);
}
notify_property_list_changed();
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
bool ChainIK3D::is_end_bone_extended(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
return chain_settings[p_index]->extend_end_bone;
}
void ChainIK3D::set_end_bone_direction(int p_index, BoneDirection p_bone_direction) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
chain_settings[p_index]->end_bone_direction = p_bone_direction;
_make_simulation_dirty(p_index);
Skeleton3D *sk = get_skeleton();
if (sk && !chain_settings[p_index]->joints.is_empty()) {
_validate_axis(sk, p_index, chain_settings[p_index]->joints.size() - 1);
}
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
SkeletonModifier3D::BoneDirection ChainIK3D::get_end_bone_direction(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), BONE_DIRECTION_FROM_PARENT);
return chain_settings[p_index]->end_bone_direction;
}
void ChainIK3D::set_end_bone_length(int p_index, float p_length) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
chain_settings[p_index]->end_bone_length = p_length;
_make_simulation_dirty(p_index);
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
float ChainIK3D::get_end_bone_length(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), 0);
return chain_settings[p_index]->end_bone_length;
}
// Individual joints.
void ChainIK3D::set_joint_bone_name(int p_index, int p_joint, const String &p_bone_name) {
// Exists only for indicate bone name on the inspector, no needs to make dirty joint array.
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<BoneJoint> &joints = chain_settings[p_index]->joints;
ERR_FAIL_INDEX(p_joint, (int)joints.size());
joints[p_joint].name = p_bone_name;
Skeleton3D *sk = get_skeleton();
if (sk) {
set_joint_bone(p_index, p_joint, sk->find_bone(joints[p_joint].name));
}
}
String ChainIK3D::get_joint_bone_name(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
const LocalVector<BoneJoint> &joints = chain_settings[p_index]->joints;
ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), String());
return joints[p_joint].name;
}
void ChainIK3D::set_joint_bone(int p_index, int p_joint, int p_bone) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<BoneJoint> &joints = chain_settings[p_index]->joints;
ERR_FAIL_INDEX(p_joint, (int)joints.size());
joints[p_joint].bone = p_bone;
Skeleton3D *sk = get_skeleton();
if (sk) {
if (joints[p_joint].bone <= -1 || joints[p_joint].bone >= sk->get_bone_count()) {
WARN_PRINT("Joint bone index out of range!");
joints[p_joint].bone = -1;
} else {
joints[p_joint].name = sk->get_bone_name(joints[p_joint].bone);
}
}
}
int ChainIK3D::get_joint_bone(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
const LocalVector<BoneJoint> &joints = chain_settings[p_index]->joints;
ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), -1);
return joints[p_joint].bone;
}
void ChainIK3D::set_joint_count(int p_index, int p_count) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
ERR_FAIL_COND(p_count < 0);
LocalVector<BoneJoint> &joints = chain_settings[p_index]->joints;
joints.resize(p_count);
_set_joint_count(p_index, p_count);
notify_property_list_changed();
}
void ChainIK3D::_set_joint_count(int p_index, int p_count) {
//
}
int ChainIK3D::get_joint_count(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), 0);
const LocalVector<BoneJoint> &joints = chain_settings[p_index]->joints;
return joints.size();
}
void ChainIK3D::_bind_methods() {
// Setting.
ClassDB::bind_method(D_METHOD("set_root_bone_name", "index", "bone_name"), &ChainIK3D::set_root_bone_name);
ClassDB::bind_method(D_METHOD("get_root_bone_name", "index"), &ChainIK3D::get_root_bone_name);
ClassDB::bind_method(D_METHOD("set_root_bone", "index", "bone"), &ChainIK3D::set_root_bone);
ClassDB::bind_method(D_METHOD("get_root_bone", "index"), &ChainIK3D::get_root_bone);
ClassDB::bind_method(D_METHOD("set_end_bone_name", "index", "bone_name"), &ChainIK3D::set_end_bone_name);
ClassDB::bind_method(D_METHOD("get_end_bone_name", "index"), &ChainIK3D::get_end_bone_name);
ClassDB::bind_method(D_METHOD("set_end_bone", "index", "bone"), &ChainIK3D::set_end_bone);
ClassDB::bind_method(D_METHOD("get_end_bone", "index"), &ChainIK3D::get_end_bone);
ClassDB::bind_method(D_METHOD("set_extend_end_bone", "index", "enabled"), &ChainIK3D::set_extend_end_bone);
ClassDB::bind_method(D_METHOD("is_end_bone_extended", "index"), &ChainIK3D::is_end_bone_extended);
ClassDB::bind_method(D_METHOD("set_end_bone_direction", "index", "bone_direction"), &ChainIK3D::set_end_bone_direction);
ClassDB::bind_method(D_METHOD("get_end_bone_direction", "index"), &ChainIK3D::get_end_bone_direction);
ClassDB::bind_method(D_METHOD("set_end_bone_length", "index", "length"), &ChainIK3D::set_end_bone_length);
ClassDB::bind_method(D_METHOD("get_end_bone_length", "index"), &ChainIK3D::get_end_bone_length);
// Individual joints.
ClassDB::bind_method(D_METHOD("get_joint_bone_name", "index", "joint"), &ChainIK3D::get_joint_bone_name);
ClassDB::bind_method(D_METHOD("get_joint_bone", "index", "joint"), &ChainIK3D::get_joint_bone);
ClassDB::bind_method(D_METHOD("get_joint_count", "index"), &ChainIK3D::get_joint_count);
}
void ChainIK3D::_validate_bone_names() {
for (uint32_t i = 0; i < settings.size(); i++) {
// Prior bone name.
if (!chain_settings[i]->root_bone.name.is_empty()) {
set_root_bone_name(i, chain_settings[i]->root_bone.name);
} else if (chain_settings[i]->root_bone.bone != -1) {
set_root_bone(i, chain_settings[i]->root_bone.bone);
}
// Prior bone name.
if (!chain_settings[i]->end_bone.name.is_empty()) {
set_end_bone_name(i, chain_settings[i]->end_bone.name);
} else if (chain_settings[i]->end_bone.bone != -1) {
set_end_bone(i, chain_settings[i]->end_bone.bone);
}
}
}
void ChainIK3D::_validate_axes(Skeleton3D *p_skeleton) const {
for (uint32_t i = 0; i < settings.size(); i++) {
for (uint32_t j = 0; j < chain_settings[i]->joints.size(); j++) {
_validate_axis(p_skeleton, i, j);
}
}
}
void ChainIK3D::_validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const {
//
}
void ChainIK3D::_make_all_joints_dirty() {
for (uint32_t i = 0; i < settings.size(); i++) {
_update_joints(i);
}
}
void ChainIK3D::_update_joints(int p_index) {
_make_simulation_dirty(p_index);
#ifdef TOOLS_ENABLED
update_gizmos(); // To clear invalid setting.
#endif // TOOLS_ENABLED
Skeleton3D *sk = get_skeleton();
int current_bone = chain_settings[p_index]->end_bone.bone;
int root_bone = chain_settings[p_index]->root_bone.bone;
if (!sk || current_bone < 0 || root_bone < 0) {
set_joint_count(p_index, 0);
return;
}
// Validation.
bool valid = false;
while (current_bone >= 0) {
if (current_bone == root_bone) {
valid = true;
break;
}
current_bone = sk->get_bone_parent(current_bone);
}
if (!valid) {
set_joint_count(p_index, 0);
ERR_FAIL_EDMSG("End bone must be the same as or a child of the root bone.");
}
Vector<int> new_joints;
current_bone = chain_settings[p_index]->end_bone.bone;
while (current_bone != root_bone) {
new_joints.push_back(current_bone);
current_bone = sk->get_bone_parent(current_bone);
}
new_joints.push_back(current_bone);
new_joints.reverse();
set_joint_count(p_index, new_joints.size());
for (uint32_t i = 0; i < new_joints.size(); i++) {
set_joint_bone(p_index, i, new_joints[i]);
}
if (sk) {
_validate_axes(sk);
}
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
void ChainIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
//
}
ChainIK3D::~ChainIK3D() {
clear_settings();
}

243
scene/3d/chain_ik_3d.h Normal file
View file

@ -0,0 +1,243 @@
/**************************************************************************/
/* chain_ik_3d.h */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#pragma once
#include "scene/3d/ik_modifier_3d.h"
class ChainIK3D : public IKModifier3D {
GDCLASS(ChainIK3D, IKModifier3D);
public:
struct ChainIK3DSetting : public IKModifier3DSetting {
BoneJoint root_bone;
BoneJoint end_bone;
// To make virtual end joint.
bool extend_end_bone = false;
BoneDirection end_bone_direction = BONE_DIRECTION_FROM_PARENT;
float end_bone_length = 0.0;
LocalVector<BoneJoint> joints;
LocalVector<IKModifier3DSolverInfo *> solver_info_list;
LocalVector<Vector3> chain;
// Only update chain coordinates to avoid to override previous result (bone poses).
// Chain coordinates will be converted to bone pose by child class cache_current_joint_rotations() in the end of iterating.
void update_chain_coordinate(Skeleton3D *p_skeleton, int p_index, const Vector3 &p_position) {
// Don't update if the position is same as the current position ().
// But distance_squared_to() is unsuitable because converting position to rotation requires a certain level of precision.
if (Math::is_zero_approx(chain[p_index].distance_to(p_position))) {
return;
}
// Allow flipping.
chain[p_index] = p_position;
cache_current_vector(p_skeleton, p_index);
}
void update_chain_coordinate_bw(Skeleton3D *p_skeleton, int p_index, const Vector3 &p_position) {
// Don't update if the position is same as the current position.
// But distance_squared_to() is unsuitable because converting position to rotation requires a certain level of precision.
if (Math::is_zero_approx(chain[p_index].distance_to(p_position))) {
return;
}
// Prevent flipping from backwards.
Vector3 result = p_position;
int HEAD = p_index - 1;
int TAIL = p_index;
if (HEAD >= 0 && HEAD < (int)solver_info_list.size()) {
IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
if (solver_info) {
Vector3 old_head_to_tail = solver_info->current_vector;
Vector3 new_head_to_tail = (result - chain[HEAD]).normalized();
if (Math::is_equal_approx((double)old_head_to_tail.dot(new_head_to_tail), -1.0)) {
chain[TAIL] = chain[HEAD] + old_head_to_tail * solver_info->length; // Revert.
return; // No change, cache is not updated.
}
}
}
chain[p_index] = result;
cache_current_vector(p_skeleton, p_index);
}
void update_chain_coordinate_fw(Skeleton3D *p_skeleton, int p_index, const Vector3 &p_position) {
// Don't update if the position is same as the current position.
// But distance_squared_to() is unsuitable because converting position to rotation requires a certain level of precision.
if (Math::is_zero_approx(chain[p_index].distance_to(p_position))) {
return;
}
// Prevent flipping from forwards.
Vector3 result = p_position;
int HEAD = p_index;
int TAIL = p_index + 1;
if (TAIL >= 0 && TAIL < (int)solver_info_list.size()) {
IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
if (solver_info) {
Vector3 old_head_to_tail = solver_info->current_vector;
Vector3 new_head_to_tail = (chain[TAIL] - result).normalized();
if (Math::is_equal_approx((double)old_head_to_tail.dot(new_head_to_tail), -1.0)) {
chain[HEAD] = chain[TAIL] - old_head_to_tail * solver_info->length; // Revert.
return; // No change, cache is not updated.
}
}
}
chain[p_index] = result;
cache_current_vector(p_skeleton, p_index);
}
void cache_current_vector(Skeleton3D *p_skeleton, int p_index) {
int cur_head = p_index - 1;
int cur_tail = p_index;
if (cur_head >= 0) {
solver_info_list[cur_head]->current_vector = (chain[cur_tail] - chain[cur_head]).normalized();
}
cur_head = p_index;
cur_tail = p_index + 1;
if (cur_tail < (int)chain.size()) {
solver_info_list[cur_head]->current_vector = (chain[cur_tail] - chain[cur_head]).normalized();
}
}
void cache_current_vectors(Skeleton3D *p_skeleton) {
for (uint32_t i = 0; i < joints.size(); i++) {
int HEAD = i;
int TAIL = i + 1;
IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
if (!solver_info) {
continue;
}
solver_info->current_vector = (chain[TAIL] - chain[HEAD]).normalized();
}
}
void init_current_joint_rotations(Skeleton3D *p_skeleton) {
if (root_bone.bone < 0) {
return;
}
Quaternion parent_gpose;
int parent = p_skeleton->get_bone_parent(root_bone.bone);
if (parent >= 0) {
parent_gpose = p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion();
}
for (uint32_t i = 0; i < joints.size(); i++) {
IKModifier3DSolverInfo *solver_info = solver_info_list[i];
if (!solver_info) {
continue;
}
solver_info->current_lrest = p_skeleton->get_bone_pose(joints[i].bone).basis.get_rotation_quaternion();
solver_info->current_grest = parent_gpose * solver_info->current_lrest;
solver_info->current_grest.normalize();
solver_info->current_lpose = p_skeleton->get_bone_pose(joints[i].bone).basis.get_rotation_quaternion();
solver_info->current_gpose = parent_gpose * solver_info->current_lpose;
solver_info->current_gpose.normalize();
parent_gpose = solver_info->current_gpose;
}
cache_current_vectors(p_skeleton);
}
~ChainIK3DSetting() {
for (uint32_t i = 0; i < solver_info_list.size(); i++) {
if (solver_info_list[i]) {
memdelete(solver_info_list[i]);
solver_info_list[i] = nullptr;
}
}
solver_info_list.clear();
}
};
protected:
LocalVector<ChainIK3DSetting *> chain_settings; // For caching.
bool _get(const StringName &p_path, Variant &r_ret) const;
bool _set(const StringName &p_path, const Variant &p_value);
void get_property_list(List<PropertyInfo> *p_list) const;
void _validate_dynamic_prop(PropertyInfo &p_property) const;
static void _bind_methods();
virtual void _validate_bone_names() override;
void _validate_axes(Skeleton3D *p_skeleton) const;
virtual void _validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const;
virtual void _make_all_joints_dirty() override;
virtual void _update_joints(int p_index) override;
virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override;
virtual void _set_joint_count(int p_index, int p_count);
public:
virtual void set_setting_count(int p_count) override {
_set_setting_count<ChainIK3DSetting>(p_count);
chain_settings = _cast_settings<ChainIK3DSetting>();
}
virtual void clear_settings() override {
_set_setting_count<ChainIK3DSetting>(0);
chain_settings.clear();
}
// Setting.
void set_root_bone_name(int p_index, const String &p_bone_name);
String get_root_bone_name(int p_index) const;
void set_root_bone(int p_index, int p_bone);
int get_root_bone(int p_index) const;
void set_end_bone_name(int p_index, const String &p_bone_name);
String get_end_bone_name(int p_index) const;
void set_end_bone(int p_index, int p_bone);
int get_end_bone(int p_index) const;
void set_extend_end_bone(int p_index, bool p_enabled);
bool is_end_bone_extended(int p_index) const;
void set_end_bone_direction(int p_index, BoneDirection p_bone_direction);
BoneDirection get_end_bone_direction(int p_index) const;
void set_end_bone_length(int p_index, float p_length);
float get_end_bone_length(int p_index) const;
// Individual joints.
void set_joint_bone_name(int p_index, int p_joint, const String &p_bone_name);
String get_joint_bone_name(int p_index, int p_joint) const;
void set_joint_bone(int p_index, int p_joint, int p_bone);
int get_joint_bone(int p_index, int p_joint) const;
void set_joint_count(int p_index, int p_count);
int get_joint_count(int p_index) const;
~ChainIK3D();
};

87
scene/3d/fabr_ik_3d.cpp Normal file
View file

@ -0,0 +1,87 @@
/**************************************************************************/
/* fabr_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 "fabr_ik_3d.h"
void FABRIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
int joint_size = (int)p_setting->joints.size();
// Backwards.
bool first = true;
for (int i = joint_size - 1; i >= 0; i--) {
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
int HEAD = i;
int TAIL = i + 1;
if (first) {
p_setting->update_chain_coordinate_bw(p_skeleton, TAIL, p_destination);
first = false;
}
p_setting->update_chain_coordinate_bw(p_skeleton, HEAD, limit_length(p_setting->chain[TAIL], p_setting->chain[HEAD], solver_info->length));
if (p_setting->joint_settings[HEAD]->rotation_axis != ROTATION_AXIS_ALL) {
p_setting->update_chain_coordinate_bw(p_skeleton, HEAD, p_setting->chain[TAIL] + p_setting->joint_settings[HEAD]->get_projected_rotation(solver_info->current_grest, p_setting->chain[HEAD] - p_setting->chain[TAIL]));
}
if (p_setting->joint_settings[HEAD]->limitation.is_valid()) {
p_setting->update_chain_coordinate_bw(p_skeleton, HEAD, p_setting->chain[TAIL] + p_setting->joint_settings[HEAD]->get_limited_rotation(solver_info->current_grest, p_setting->chain[HEAD] - p_setting->chain[TAIL], solver_info->forward_vector));
}
}
// Forwards.
first = true;
for (int i = 0; i < joint_size; i++) {
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
int HEAD = i;
int TAIL = i + 1;
if (first) {
p_setting->update_chain_coordinate_fw(p_skeleton, HEAD, p_skeleton->get_bone_global_pose(p_setting->joints[HEAD].bone).origin);
first = false;
}
p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, limit_length(p_setting->chain[HEAD], p_setting->chain[TAIL], solver_info->length));
if (p_setting->joint_settings[HEAD]->rotation_axis != ROTATION_AXIS_ALL) {
p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, p_setting->chain[HEAD] + p_setting->joint_settings[HEAD]->get_projected_rotation(solver_info->current_grest, p_setting->chain[TAIL] - p_setting->chain[HEAD]));
}
if (p_setting->joint_settings[HEAD]->limitation.is_valid()) {
p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, p_setting->chain[HEAD] + p_setting->joint_settings[HEAD]->get_limited_rotation(solver_info->current_grest, p_setting->chain[TAIL] - p_setting->chain[HEAD], solver_info->forward_vector));
}
}
}

40
scene/3d/fabr_ik_3d.h Normal file
View file

@ -0,0 +1,40 @@
/**************************************************************************/
/* fabr_ik_3d.h */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#pragma once
#include "scene/3d/iterate_ik_3d.h"
class FABRIK3D : public IterateIK3D {
GDCLASS(FABRIK3D, IterateIK3D);
protected:
virtual void _solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) override;
};

151
scene/3d/ik_modifier_3d.cpp Normal file
View file

@ -0,0 +1,151 @@
/**************************************************************************/
/* ik_modifier_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 "ik_modifier_3d.h"
void IKModifier3D::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_ENTER_TREE: {
#ifdef TOOLS_ENABLED
if (Engine::get_singleton()->is_editor_hint()) {
set_notify_local_transform(true); // Used for updating gizmo in editor.
}
#endif // TOOLS_ENABLED
_make_all_joints_dirty();
} break;
#ifdef TOOLS_ENABLED
case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
update_gizmos();
} break;
#endif // TOOLS_ENABLED
}
}
void IKModifier3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_setting_count", "count"), &IKModifier3D::set_setting_count);
ClassDB::bind_method(D_METHOD("get_setting_count"), &IKModifier3D::get_setting_count);
ClassDB::bind_method(D_METHOD("clear_settings"), &IKModifier3D::clear_settings);
// To process manually.
ClassDB::bind_method(D_METHOD("reset"), &IKModifier3D::reset);
}
void IKModifier3D::_set_active(bool p_active) {
if (p_active) {
reset();
}
}
void IKModifier3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) {
if (p_old && p_old->is_connected(SNAME("rest_updated"), callable_mp(this, &IKModifier3D::_make_all_joints_dirty))) {
p_old->disconnect(SNAME("rest_updated"), callable_mp(this, &IKModifier3D::_make_all_joints_dirty));
}
if (p_new && !p_new->is_connected(SNAME("rest_updated"), callable_mp(this, &IKModifier3D::_make_all_joints_dirty))) {
p_new->connect(SNAME("rest_updated"), callable_mp(this, &IKModifier3D::_make_all_joints_dirty));
}
_make_all_joints_dirty();
}
void IKModifier3D::_validate_bone_names() {
//
}
void IKModifier3D::_make_all_joints_dirty() {
//
}
void IKModifier3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
//
}
void IKModifier3D::_update_joints(int p_index) {
//
}
void IKModifier3D::_make_simulation_dirty(int p_index) {
//
}
void IKModifier3D::_process_modification(double p_delta) {
Skeleton3D *skeleton = get_skeleton();
if (!skeleton) {
return;
}
_process_ik(skeleton, p_delta);
}
void IKModifier3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
//
}
Quaternion IKModifier3D::get_local_pose_rotation(Skeleton3D *p_skeleton, int p_bone, const Quaternion &p_global_pose_rotation) {
int parent = p_skeleton->get_bone_parent(p_bone);
if (parent < 0) {
return p_global_pose_rotation;
}
return p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion().inverse() * p_global_pose_rotation;
}
Vector3 IKModifier3D::get_bone_axis(int p_end_bone, BoneDirection p_direction) const {
if (!is_inside_tree()) {
return Vector3();
}
Vector3 axis;
if (p_direction == BONE_DIRECTION_FROM_PARENT) {
Skeleton3D *sk = get_skeleton();
if (sk) {
axis = sk->get_bone_rest(p_end_bone).basis.xform_inv(sk->get_bone_rest(p_end_bone).origin);
axis.normalize();
}
} else {
axis = get_vector_from_bone_axis(static_cast<BoneAxis>((int)p_direction));
}
return axis;
}
int IKModifier3D::get_setting_count() const {
return settings.size();
}
void IKModifier3D::reset() {
Skeleton3D *skeleton = get_skeleton();
if (!skeleton) {
return;
}
for (uint32_t i = 0; i < settings.size(); i++) {
_make_simulation_dirty(i);
_init_joints(skeleton, i);
}
}
IKModifier3D::~IKModifier3D() {
clear_settings();
}

132
scene/3d/ik_modifier_3d.h Normal file
View file

@ -0,0 +1,132 @@
/**************************************************************************/
/* ik_modifier_3d.h */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#pragma once
#include "scene/3d/skeleton_modifier_3d.h"
class IKModifier3D : public SkeletonModifier3D {
GDCLASS(IKModifier3D, SkeletonModifier3D);
protected:
#ifdef TOOLS_ENABLED
bool saving = false;
#endif // TOOLS_ENABLED
Transform3D cached_space;
bool joints_dirty = false;
public:
struct BoneJoint {
StringName name;
int bone = -1;
};
struct IKModifier3DSolverInfo {
Quaternion current_lpose;
Quaternion current_lrest;
Quaternion current_gpose;
Quaternion current_grest;
Vector3 current_vector; // Global so needs xfrom_inv by gpose or grest in the process.
Vector3 forward_vector; // Local.
float length = 0.0;
};
struct IKModifier3DSetting {
bool simulation_dirty = true;
bool joints_dirty = false;
};
protected:
LocalVector<IKModifier3DSetting *> settings;
void _notification(int p_what);
static void _bind_methods();
virtual void _set_active(bool p_active) override;
virtual void _skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) override;
virtual void _validate_bone_names() override;
virtual void _make_all_joints_dirty();
virtual void _init_joints(Skeleton3D *p_skeleton, int p_index);
virtual void _update_joints(int p_index);
virtual void _make_simulation_dirty(int p_index);
virtual void _process_modification(double p_delta) override;
virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta);
template <typename T>
void _set_setting_count(int p_count) {
ERR_FAIL_COND(p_count < 0);
int delta = p_count - settings.size();
if (delta < 0) {
for (int i = delta; i < 0; i++) {
memdelete(static_cast<T *>(settings[settings.size() + i]));
settings[settings.size() + i] = nullptr;
}
}
settings.resize(p_count);
delta++;
if (delta > 1) {
for (int i = 1; i < delta; i++) {
settings[p_count - i] = memnew(T);
}
}
notify_property_list_changed();
}
template <typename T>
LocalVector<T *> _cast_settings() const {
LocalVector<T *> result;
for (uint32_t i = 0; i < settings.size(); i++) {
result.push_back(static_cast<T *>(settings[i]));
}
return result;
}
public:
int get_setting_count() const;
virtual void set_setting_count(int p_count) {
_set_setting_count<IKModifier3DSetting>(p_count);
}
virtual void clear_settings() {
_set_setting_count<IKModifier3DSetting>(0);
}
// Helper.
static Quaternion get_local_pose_rotation(Skeleton3D *p_skeleton, int p_bone, const Quaternion &p_global_pose_rotation);
Vector3 get_bone_axis(int p_end_bone, BoneDirection p_direction) const;
// To process manually.
void reset();
~IKModifier3D();
};

554
scene/3d/iterate_ik_3d.cpp Normal file
View file

@ -0,0 +1,554 @@
/**************************************************************************/
/* iterate_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 "iterate_ik_3d.h"
bool IterateIK3D::_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 == "joints") {
int idx = path.get_slicec('/', 3).to_int();
String prop = path.get_slicec('/', 4);
if (prop == "rotation_axis") {
set_joint_rotation_axis(which, idx, static_cast<RotationAxis>((int)p_value));
} else if (prop == "rotation_axis_vector") {
set_joint_rotation_axis_vector(which, idx, p_value);
} else if (prop == "limitation") {
String opt = path.get_slicec('/', 5);
if (opt.is_empty()) {
set_joint_limitation(which, idx, p_value);
} else if (opt == "right_axis") {
set_joint_limitation_right_axis(which, idx, p_value);
} else if (opt == "right_axis_vector") {
set_joint_limitation_right_axis_vector(which, idx, p_value);
} else if (opt == "rotation_offset") {
set_joint_limitation_rotation_offset(which, idx, p_value);
} else {
return false;
}
} else {
return false;
}
} else {
return false;
}
}
return true;
}
bool IterateIK3D::_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 == "joints") {
int idx = path.get_slicec('/', 3).to_int();
String prop = path.get_slicec('/', 4);
if (prop == "rotation_axis") {
r_ret = (int)get_joint_rotation_axis(which, idx);
} else if (prop == "rotation_axis_vector") {
r_ret = get_joint_rotation_axis_vector(which, idx);
} else if (prop == "limitation") {
String opt = path.get_slicec('/', 5);
if (opt.is_empty()) {
r_ret = get_joint_limitation(which, idx);
} else if (opt == "right_axis") {
r_ret = get_joint_limitation_right_axis(which, idx);
} else if (opt == "right_axis_vector") {
r_ret = get_joint_limitation_right_axis_vector(which, idx);
} else if (opt == "rotation_offset") {
r_ret = get_joint_limitation_rotation_offset(which, idx);
} else {
return false;
}
} else {
return false;
}
} else {
return false;
}
}
return true;
}
void IterateIK3D::_get_property_list(List<PropertyInfo> *p_list) const {
LocalVector<PropertyInfo> props;
for (uint32_t i = 0; i < settings.size(); i++) {
String path = "settings/" + itos(i) + "/";
p_list->push_back(PropertyInfo(Variant::NODE_PATH, path + "target_node"));
for (uint32_t j = 0; j < iterate_settings[i]->joints.size(); j++) {
String joint_path = path + "joints/" + itos(j) + "/";
props.push_back(PropertyInfo(Variant::INT, joint_path + "rotation_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_rotation_axis()));
props.push_back(PropertyInfo(Variant::VECTOR3, joint_path + "rotation_axis_vector"));
props.push_back(PropertyInfo(Variant::OBJECT, joint_path + "limitation", PROPERTY_HINT_RESOURCE_TYPE, "JointLimitation3D"));
props.push_back(PropertyInfo(Variant::INT, joint_path + "limitation/right_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_secondary_direction()));
props.push_back(PropertyInfo(Variant::VECTOR3, joint_path + "limitation/right_axis_vector"));
props.push_back(PropertyInfo(Variant::QUATERNION, joint_path + "limitation/rotation_offset"));
}
}
ChainIK3D::get_property_list(p_list);
for (PropertyInfo &p : props) {
_validate_dynamic_prop(p);
p_list->push_back(p);
}
}
void IterateIK3D::_validate_dynamic_prop(PropertyInfo &p_property) const {
PackedStringArray split = p_property.name.split("/");
if (split.size() > 3 && split[0] == "settings") {
int which = split[1].to_int();
int joint = split[3].to_int();
// Joints option.
if (split[2] == "joints" && split.size() > 4) {
if (split[4] == "rotation_axis_vector" && get_joint_rotation_axis(which, joint) != ROTATION_AXIS_CUSTOM) {
p_property.usage = PROPERTY_USAGE_NONE;
}
if (split[4] == "limitation" && split.size() > 5) {
if (get_joint_limitation(which, joint).is_null()) {
p_property.usage = PROPERTY_USAGE_NONE;
} else if (split[5] == "right_axis_vector" && get_joint_limitation_right_axis(which, joint) != SECONDARY_DIRECTION_CUSTOM) {
p_property.usage = PROPERTY_USAGE_NONE;
}
}
}
}
}
PackedStringArray IterateIK3D::get_configuration_warnings() const {
PackedStringArray warnings = SkeletonModifier3D::get_configuration_warnings();
for (uint32_t i = 0; i < iterate_settings.size(); i++) {
if (iterate_settings[i]->target_node.is_empty()) {
warnings.push_back(RTR("Detecting settings with no target set! IterateIK3D must have a target to work."));
break;
}
}
return warnings;
}
void IterateIK3D::set_max_iterations(int p_max_iterations) {
max_iterations = p_max_iterations;
}
int IterateIK3D::get_max_iterations() const {
return max_iterations;
}
void IterateIK3D::set_min_distance(double p_min_distance) {
min_distance = p_min_distance;
}
double IterateIK3D::get_min_distance() const {
return min_distance;
}
void IterateIK3D::set_angular_delta_limit(double p_angular_delta_limit) {
angular_delta_limit = p_angular_delta_limit;
}
double IterateIK3D::get_angular_delta_limit() const {
return angular_delta_limit;
}
// Setting.
void IterateIK3D::set_target_node(int p_index, const NodePath &p_node_path) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
iterate_settings[p_index]->target_node = p_node_path;
update_configuration_warnings();
}
NodePath IterateIK3D::get_target_node(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
return iterate_settings[p_index]->target_node;
}
// Individual joints.
void IterateIK3D::set_joint_rotation_axis(int p_index, int p_joint, RotationAxis p_axis) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->rotation_axis = p_axis;
Skeleton3D *sk = get_skeleton();
if (sk) {
_validate_axis(sk, p_index, p_joint);
}
notify_property_list_changed();
iterate_settings[p_index]->simulation_dirty = true; // Snapping to planes is needed in the initialization, so need to restructure.
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
SkeletonModifier3D::RotationAxis IterateIK3D::get_joint_rotation_axis(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), ROTATION_AXIS_ALL);
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), ROTATION_AXIS_ALL);
return joint_settings[p_joint]->rotation_axis;
}
void IterateIK3D::set_joint_rotation_axis_vector(int p_index, int p_joint, const Vector3 &p_vector) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->rotation_axis_vector = p_vector;
Skeleton3D *sk = get_skeleton();
if (sk) {
_validate_axis(sk, p_index, p_joint);
}
iterate_settings[p_index]->simulation_dirty = true; // Snapping to planes is needed in the initialization, so need to restructure.
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
Vector3 IterateIK3D::get_joint_rotation_axis_vector(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Vector3());
return joint_settings[p_joint]->get_rotation_axis_vector();
}
Quaternion IterateIK3D::get_joint_limitation_space(int p_index, int p_joint, const Vector3 &p_forward) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Quaternion());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Quaternion());
return joint_settings[p_joint]->get_limitation_space(p_forward);
}
void IterateIK3D::set_joint_limitation(int p_index, int p_joint, const Ref<JointLimitation3D> &p_limitation) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
_unbind_joint_limitation(p_index, p_joint);
joint_settings[p_joint]->limitation = p_limitation;
_bind_joint_limitation(p_index, p_joint);
notify_property_list_changed();
_update_joint_limitation(p_index, p_joint);
}
Ref<JointLimitation3D> IterateIK3D::get_joint_limitation(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Ref<JointLimitation3D>());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Ref<JointLimitation3D>());
return joint_settings[p_joint]->limitation;
}
void IterateIK3D::set_joint_limitation_right_axis(int p_index, int p_joint, SecondaryDirection p_direction) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->limitation_right_axis = p_direction;
notify_property_list_changed();
_update_joint_limitation(p_index, p_joint);
}
IKModifier3D::SecondaryDirection IterateIK3D::get_joint_limitation_right_axis(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), SECONDARY_DIRECTION_NONE);
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), SECONDARY_DIRECTION_NONE);
return joint_settings[p_joint]->limitation_right_axis;
}
void IterateIK3D::set_joint_limitation_right_axis_vector(int p_index, int p_joint, const Vector3 &p_vector) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->limitation_right_axis_vector = p_vector;
_update_joint_limitation(p_index, p_joint);
}
Vector3 IterateIK3D::get_joint_limitation_right_axis_vector(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Vector3());
return joint_settings[p_joint]->get_limitation_right_axis_vector();
}
void IterateIK3D::set_joint_limitation_rotation_offset(int p_index, int p_joint, const Quaternion &p_offset) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->limitation_rotation_offset = p_offset;
_update_joint_limitation(p_index, p_joint);
}
Quaternion IterateIK3D::get_joint_limitation_rotation_offset(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Quaternion());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Quaternion());
return joint_settings[p_joint]->limitation_rotation_offset;
}
void IterateIK3D::_set_joint_count(int p_index, int p_count) {
_unbind_joint_limitations(p_index);
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
int delta = p_count - joint_settings.size();
if (delta < 0) {
for (int i = delta; i < 0; i++) {
memdelete(joint_settings[joint_settings.size() + i]);
joint_settings[joint_settings.size() + i] = nullptr;
}
}
joint_settings.resize(p_count);
delta++;
if (delta > 1) {
for (int i = 1; i < delta; i++) {
joint_settings[p_count - i] = memnew(IterateIK3DJointSetting);
}
}
}
void IterateIK3D::_validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const {
RotationAxis axis = iterate_settings[p_index]->joint_settings[p_joint]->rotation_axis;
if (axis == ROTATION_AXIS_ALL) {
return;
}
Vector3 rot = get_joint_rotation_axis_vector(p_index, p_joint).normalized();
Vector3 fwd;
if (p_joint < (int)iterate_settings[p_index]->joints.size() - 1) {
fwd = p_skeleton->get_bone_rest(iterate_settings[p_index]->joints[p_joint + 1].bone).origin;
} else if (iterate_settings[p_index]->extend_end_bone) {
fwd = get_bone_axis(iterate_settings[p_index]->end_bone.bone, iterate_settings[p_index]->end_bone_direction);
if (fwd.is_zero_approx()) {
return;
}
}
fwd.normalize();
if (Math::is_equal_approx(Math::abs(rot.dot(fwd)), 1)) {
WARN_PRINT_ED("Setting: " + itos(p_index) + " Joint: " + itos(p_joint) + ": Rotation axis and forward vector are colinear. This is not advised as it may cause unwanted rotation.");
}
}
void IterateIK3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_max_iterations", "max_iterations"), &IterateIK3D::set_max_iterations);
ClassDB::bind_method(D_METHOD("get_max_iterations"), &IterateIK3D::get_max_iterations);
ClassDB::bind_method(D_METHOD("set_min_distance", "min_distance"), &IterateIK3D::set_min_distance);
ClassDB::bind_method(D_METHOD("get_min_distance"), &IterateIK3D::get_min_distance);
ClassDB::bind_method(D_METHOD("set_angular_delta_limit", "angular_delta_limit"), &IterateIK3D::set_angular_delta_limit);
ClassDB::bind_method(D_METHOD("get_angular_delta_limit"), &IterateIK3D::get_angular_delta_limit);
// Setting.
ClassDB::bind_method(D_METHOD("set_target_node", "index", "target_node"), &IterateIK3D::set_target_node);
ClassDB::bind_method(D_METHOD("get_target_node", "index"), &IterateIK3D::get_target_node);
// Individual joints.
ClassDB::bind_method(D_METHOD("set_joint_rotation_axis", "index", "joint", "axis"), &IterateIK3D::set_joint_rotation_axis);
ClassDB::bind_method(D_METHOD("get_joint_rotation_axis", "index", "joint"), &IterateIK3D::get_joint_rotation_axis);
ClassDB::bind_method(D_METHOD("set_joint_rotation_axis_vector", "index", "joint", "axis_vector"), &IterateIK3D::set_joint_rotation_axis_vector);
ClassDB::bind_method(D_METHOD("get_joint_rotation_axis_vector", "index", "joint"), &IterateIK3D::get_joint_rotation_axis_vector);
ClassDB::bind_method(D_METHOD("set_joint_limitation", "index", "joint", "limitation"), &IterateIK3D::set_joint_limitation);
ClassDB::bind_method(D_METHOD("get_joint_limitation", "index", "joint"), &IterateIK3D::get_joint_limitation);
ClassDB::bind_method(D_METHOD("set_joint_limitation_right_axis", "index", "joint", "direction"), &IterateIK3D::set_joint_limitation_right_axis);
ClassDB::bind_method(D_METHOD("get_joint_limitation_right_axis", "index", "joint"), &IterateIK3D::get_joint_limitation_right_axis);
ClassDB::bind_method(D_METHOD("set_joint_limitation_right_axis_vector", "index", "joint", "vector"), &IterateIK3D::set_joint_limitation_right_axis_vector);
ClassDB::bind_method(D_METHOD("get_joint_limitation_right_axis_vector", "index", "joint"), &IterateIK3D::get_joint_limitation_right_axis_vector);
ClassDB::bind_method(D_METHOD("set_joint_limitation_rotation_offset", "index", "joint", "offset"), &IterateIK3D::set_joint_limitation_rotation_offset);
ClassDB::bind_method(D_METHOD("get_joint_limitation_rotation_offset", "index", "joint"), &IterateIK3D::get_joint_limitation_rotation_offset);
ADD_PROPERTY(PropertyInfo(Variant::INT, "max_iterations", PROPERTY_HINT_RANGE, "0,100,or_greater"), "set_max_iterations", "get_max_iterations");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "min_distance", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater"), "set_min_distance", "get_min_distance");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "angular_delta_limit", PROPERTY_HINT_RANGE, "0,180,0.001,radians_as_degrees"), "set_angular_delta_limit", "get_angular_delta_limit");
ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
}
void IterateIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
IterateIK3DSetting *setting = iterate_settings[p_index];
cached_space = p_skeleton->get_global_transform_interpolated();
if (!setting->simulation_dirty) {
return;
}
_unbind_joint_limitations(p_index);
for (uint32_t i = 0; i < setting->solver_info_list.size(); i++) {
if (setting->solver_info_list[i]) {
memdelete(setting->solver_info_list[i]);
setting->solver_info_list[i] = nullptr;
}
}
setting->solver_info_list.clear();
setting->solver_info_list.resize_initialized(setting->joints.size());
setting->chain.clear();
bool extend_end_bone = setting->extend_end_bone && setting->end_bone_length > 0;
for (uint32_t i = 0; i < setting->joints.size(); i++) {
setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).origin);
bool last = i == setting->joints.size() - 1;
if (last && extend_end_bone && setting->end_bone_length > 0) {
Vector3 axis = get_bone_axis(setting->end_bone.bone, setting->end_bone_direction);
if (axis.is_zero_approx()) {
continue;
}
setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
setting->solver_info_list[i]->forward_vector = snap_vector_to_plane(setting->joint_settings[i]->get_rotation_axis_vector(), axis.normalized());
setting->solver_info_list[i]->length = setting->end_bone_length;
setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).xform(axis * setting->end_bone_length));
} else if (!last) {
Vector3 axis = p_skeleton->get_bone_rest(setting->joints[i + 1].bone).origin;
if (axis.is_zero_approx()) {
continue; // Means always we need to check solver info, but `!solver_info` means that the bone is zero length, so IK should skip it in the all process.
}
setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
setting->solver_info_list[i]->forward_vector = snap_vector_to_plane(setting->joint_settings[i]->get_rotation_axis_vector(), axis.normalized());
setting->solver_info_list[i]->length = axis.length();
}
}
_bind_joint_limitations(p_index);
setting->init_current_joint_rotations(p_skeleton);
setting->simulation_dirty = false;
setting->simulated = false;
}
void IterateIK3D::_make_simulation_dirty(int p_index) {
IterateIK3DSetting *setting = iterate_settings[p_index];
if (!setting) {
return;
}
setting->simulation_dirty = true;
}
void IterateIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
min_distance_squared = min_distance * min_distance;
for (uint32_t i = 0; i < settings.size(); i++) {
_init_joints(p_skeleton, i);
Node3D *target = Object::cast_to<Node3D>(get_node_or_null(iterate_settings[i]->target_node));
if (!target || iterate_settings[i]->chain.is_empty()) {
continue; // Abort.
}
iterate_settings[i]->cache_current_joint_rotations(p_skeleton); // Iterate over first to detect parent (outside of the chain) bone pose changes.
Vector3 destination = cached_space.affine_inverse().xform(target->get_global_transform_interpolated().origin);
_process_joints(p_delta, p_skeleton, iterate_settings[i], destination);
}
}
void IterateIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
double distance_to_target_sq = INFINITY;
int iteration_count = 0;
if (p_setting->is_penetrated(p_destination)) {
return;
}
// To prevent oscillation, if it has been processed at least once and target was reached, abort iterating.
if (p_setting->simulated) {
distance_to_target_sq = p_setting->chain[p_setting->chain.size() - 1].distance_squared_to(p_destination);
}
while (distance_to_target_sq > min_distance_squared && iteration_count < max_iterations) {
// Solve the IK for this iteration.
_solve_iteration(p_delta, p_skeleton, p_setting, p_destination);
// Update virtual bone rest/poses.
p_setting->cache_current_joint_rotations(p_skeleton, angular_delta_limit);
distance_to_target_sq = p_setting->chain[p_setting->chain.size() - 1].distance_squared_to(p_destination);
iteration_count++;
}
// Apply the virtual bone rest/poses to the actual bones.
for (uint32_t i = 0; i < p_setting->solver_info_list.size(); i++) {
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
p_skeleton->set_bone_pose_rotation(p_setting->joints[i].bone, solver_info->current_lpose);
}
p_setting->simulated = true;
}
void IterateIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
//
}
void IterateIK3D::_update_joint_limitation(int p_index, int p_joint) {
ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
iterate_settings[p_index]->simulated = false;
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); // p_joint is unused directly, but need to identify bound index.
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
void IterateIK3D::_bind_joint_limitation(int p_index, int p_joint) {
ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
if (joint_settings[p_joint]->limitation.is_valid()) {
joint_settings[p_joint]->limitation->connect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, p_joint));
}
}
void IterateIK3D::_unbind_joint_limitation(int p_index, int p_joint) {
ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
if (joint_settings[p_joint]->limitation.is_valid()) {
joint_settings[p_joint]->limitation->disconnect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, p_joint));
}
}
void IterateIK3D::_bind_joint_limitations(int p_index) {
for (uint32_t i = 0; i < iterate_settings[p_index]->joints.size(); i++) {
if (iterate_settings[p_index]->joint_settings[i]->limitation.is_valid()) {
iterate_settings[p_index]->joint_settings[i]->limitation->connect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, i));
}
}
}
void IterateIK3D::_unbind_joint_limitations(int p_index) {
for (uint32_t i = 0; i < iterate_settings[p_index]->joint_settings.size(); i++) {
if (iterate_settings[p_index]->joint_settings[i]->limitation.is_valid()) {
iterate_settings[p_index]->joint_settings[i]->limitation->disconnect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, i));
}
}
}
IterateIK3D::~IterateIK3D() {
for (uint32_t i = 0; i < iterate_settings.size(); i++) {
_unbind_joint_limitations(i);
}
clear_settings();
}

315
scene/3d/iterate_ik_3d.h Normal file
View file

@ -0,0 +1,315 @@
/**************************************************************************/
/* iterate_ik_3d.h */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#pragma once
#include "scene/3d/chain_ik_3d.h"
#include "scene/resources/3d/joint_limitation_3d.h"
class IterateIK3D : public ChainIK3D {
GDCLASS(IterateIK3D, ChainIK3D);
public:
struct IterateIK3DJointSetting {
// To limit rotation.
RotationAxis rotation_axis = ROTATION_AXIS_ALL;
Vector3 rotation_axis_vector = Vector3(1, 0, 0);
Ref<JointLimitation3D> limitation;
SecondaryDirection limitation_right_axis = SECONDARY_DIRECTION_NONE;
Vector3 limitation_right_axis_vector = Vector3(1, 0, 0);
Quaternion limitation_rotation_offset;
// Rotation axis.
Vector3 get_rotation_axis_vector() const {
Vector3 ret;
switch (rotation_axis) {
case ROTATION_AXIS_X:
ret = Vector3(1, 0, 0);
break;
case ROTATION_AXIS_Y:
ret = Vector3(0, 1, 0);
break;
case ROTATION_AXIS_Z:
ret = Vector3(0, 0, 1);
break;
case ROTATION_AXIS_ALL:
ret = Vector3(0, 0, 0);
break;
case ROTATION_AXIS_CUSTOM:
ret = rotation_axis_vector;
break;
}
return ret;
}
Vector3 get_limitation_right_axis_vector() const {
Vector3 ret;
switch (limitation_right_axis) {
case SECONDARY_DIRECTION_NONE:
ret = Vector3(0, 0, 0);
break;
case SECONDARY_DIRECTION_PLUS_X:
ret = Vector3(1, 0, 0);
break;
case SECONDARY_DIRECTION_MINUS_X:
ret = Vector3(-1, 0, 0);
break;
case SECONDARY_DIRECTION_PLUS_Y:
ret = Vector3(0, 1, 0);
break;
case SECONDARY_DIRECTION_MINUS_Y:
ret = Vector3(0, -1, 0);
break;
case SECONDARY_DIRECTION_PLUS_Z:
ret = Vector3(0, 0, 1);
break;
case SECONDARY_DIRECTION_MINUS_Z:
ret = Vector3(0, 0, -1);
break;
case SECONDARY_DIRECTION_CUSTOM:
ret = limitation_right_axis_vector;
break;
}
return ret;
}
Quaternion get_limitation_space(const Vector3 &p_local_forward) const {
if (limitation.is_null()) {
return Quaternion();
}
return limitation->make_space(p_local_forward, get_limitation_right_axis_vector(), limitation_rotation_offset);
}
// Get rotation around normal vector (normal vector is rotation axis).
Vector3 get_projected_rotation(const Quaternion &p_offset, const Vector3 &p_vector) const {
ERR_FAIL_COND_V(rotation_axis == ROTATION_AXIS_ALL, p_vector);
const double ALMOST_ONE = 1.0 - CMP_EPSILON;
Vector3 axis = get_rotation_axis_vector().normalized();
Vector3 local_vector = p_offset.xform_inv(p_vector);
double length = local_vector.length();
Vector3 projected = snap_vector_to_plane(axis, local_vector.normalized());
if (!Math::is_zero_approx(length)) {
projected = projected.normalized() * length;
}
if (Math::abs(local_vector.normalized().dot(axis)) > ALMOST_ONE) {
return p_vector;
}
return p_offset.xform(projected);
}
// Get limited rotation from forward axis in local rest space.
Vector3 get_limited_rotation(const Quaternion &p_offset, const Vector3 &p_vector, const Vector3 &p_forward) const {
ERR_FAIL_COND_V(limitation.is_null(), p_vector);
Vector3 local_vector = p_offset.xform_inv(p_vector);
float length = local_vector.length();
if (Math::is_zero_approx(length)) {
return p_vector;
}
Vector3 limited = limitation->solve(p_forward, get_limitation_right_axis_vector(), limitation_rotation_offset, local_vector.normalized()) * length;
return p_offset.xform(limited);
}
~IterateIK3DJointSetting() {
limitation.unref();
}
};
struct IterateIK3DSetting : public ChainIK3DSetting {
NodePath target_node;
LocalVector<IterateIK3DJointSetting *> joint_settings;
bool simulated = false;
bool is_penetrated(const Vector3 &p_destination) {
bool ret = false;
Vector3 chain_dir = (chain[chain.size() - 1] - chain[0]).normalized();
bool is_straight = true;
for (uint32_t i = 1; i < chain.size() - 1; i++) {
Vector3 dir = (chain[i] - chain[0]).normalized();
if (!dir.is_equal_approx(chain_dir)) {
is_straight = false;
break;
}
}
if (is_straight) {
Vector3 to_target = (p_destination - chain[0]);
double proj = to_target.dot(chain_dir);
double total_length = 0;
for (uint32_t i = 0; i < solver_info_list.size(); i++) {
if (solver_info_list[i]) {
total_length += solver_info_list[i]->length;
}
}
ret = proj >= 0 && proj <= total_length && (to_target.normalized().is_equal_approx(chain_dir));
}
return ret;
}
// Make rotation as bone pose from chain coordinates.
// p_extra is delta angle limitation.
void cache_current_joint_rotations(Skeleton3D *p_skeleton, double p_angular_delta_limit = Math::PI) {
Transform3D parent_gpose_tr;
int parent = p_skeleton->get_bone_parent(root_bone.bone);
if (parent >= 0) {
parent_gpose_tr = p_skeleton->get_bone_global_pose(parent);
}
Quaternion parent_gpose = parent_gpose_tr.basis.get_rotation_quaternion();
for (uint32_t i = 0; i < joints.size(); i++) {
int HEAD = i;
IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
if (!solver_info) {
continue;
}
solver_info->current_lrest = p_skeleton->get_bone_pose(joints[HEAD].bone).basis.get_rotation_quaternion();
solver_info->current_grest = parent_gpose * solver_info->current_lrest;
solver_info->current_grest.normalize();
Vector3 from = solver_info->forward_vector;
Vector3 to = solver_info->current_grest.xform_inv(solver_info->current_vector).normalized();
Quaternion prev = solver_info->current_lpose;
if (joint_settings[HEAD]->rotation_axis == ROTATION_AXIS_ALL) {
solver_info->current_lpose = solver_info->current_lrest * get_swing(Quaternion(from, to), from);
} else {
// To stabilize rotation path especially nearely 180deg.
solver_info->current_lpose = solver_info->current_lrest * get_from_to_rotation_by_axis(from, to, joint_settings[HEAD]->get_rotation_axis_vector().normalized());
}
double diff = prev.angle_to(solver_info->current_lpose);
if (!Math::is_zero_approx(diff)) {
solver_info->current_lpose = prev.slerp(solver_info->current_lpose, MIN(1.0, p_angular_delta_limit / diff));
}
solver_info->current_gpose = parent_gpose * solver_info->current_lpose;
solver_info->current_gpose.normalize();
parent_gpose = solver_info->current_gpose;
}
// Apply back angular_delta_limit to chain coordinates.
if (chain.is_empty()) {
return;
}
chain[0] = p_skeleton->get_bone_global_pose(root_bone.bone).origin;
for (uint32_t i = 0; i < solver_info_list.size(); i++) {
int HEAD = i;
int TAIL = i + 1;
IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
if (!solver_info) {
continue;
}
chain[TAIL] = chain[HEAD] + solver_info->current_gpose.xform(solver_info->forward_vector) * solver_info->length;
}
cache_current_vectors(p_skeleton);
}
~IterateIK3DSetting() {
for (uint32_t i = 0; i < joint_settings.size(); i++) {
if (joint_settings[i]) {
memdelete(joint_settings[i]);
joint_settings[i] = nullptr;
}
}
joint_settings.clear();
}
};
protected:
LocalVector<IterateIK3DSetting *> iterate_settings; // For caching.
int max_iterations = 4;
double min_distance = 0.001; // If distance between end joint and target is less than min_distance, finish iteration.
double min_distance_squared = min_distance * min_distance; // For cache.
double angular_delta_limit = Math::deg_to_rad(2.0); // If the delta is too large, the results before and after iterating can change significantly, and divergence of calculations can easily occur.
bool _get(const StringName &p_path, Variant &r_ret) const;
bool _set(const StringName &p_path, const Variant &p_value);
void _get_property_list(List<PropertyInfo> *p_list) const;
void _validate_dynamic_prop(PropertyInfo &p_property) const;
static void _bind_methods();
virtual void _validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const override;
virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override;
virtual void _make_simulation_dirty(int p_index) override;
virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override;
void _process_joints(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_target_destination);
virtual void _solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination);
virtual void _set_joint_count(int p_index, int p_count) override;
void _update_joint_limitation(int p_index, int p_joint);
void _bind_joint_limitation(int p_index, int p_joint);
void _unbind_joint_limitation(int p_index, int p_joint);
void _bind_joint_limitations(int p_index);
void _unbind_joint_limitations(int p_index);
public:
virtual PackedStringArray get_configuration_warnings() const override;
virtual void set_setting_count(int p_count) override {
_set_setting_count<IterateIK3DSetting>(p_count);
iterate_settings = _cast_settings<IterateIK3DSetting>();
chain_settings = _cast_settings<ChainIK3DSetting>(); // Don't forget to sync super class settings.
}
virtual void clear_settings() override {
_set_setting_count<IterateIK3DSetting>(0);
iterate_settings.clear();
chain_settings.clear(); // Don't forget to sync super class settings.
}
void set_max_iterations(int p_max_iterations);
int get_max_iterations() const;
void set_min_distance(double p_min_distance);
double get_min_distance() const;
void set_angular_delta_limit(double p_angular_delta_limit);
double get_angular_delta_limit() const;
// Setting.
void set_target_node(int p_index, const NodePath &p_target_node);
NodePath get_target_node(int p_index) const;
// Individual joints.
void set_joint_rotation_axis(int p_index, int p_joint, RotationAxis p_axis);
RotationAxis get_joint_rotation_axis(int p_index, int p_joint) const;
void set_joint_rotation_axis_vector(int p_index, int p_joint, const Vector3 &p_vector);
Vector3 get_joint_rotation_axis_vector(int p_index, int p_joint) const;
void set_joint_limitation(int p_index, int p_joint, const Ref<JointLimitation3D> &p_limitation);
Ref<JointLimitation3D> get_joint_limitation(int p_index, int p_joint) const;
void set_joint_limitation_right_axis(int p_index, int p_joint, SecondaryDirection p_direction);
SecondaryDirection get_joint_limitation_right_axis(int p_index, int p_joint) const;
void set_joint_limitation_right_axis_vector(int p_index, int p_joint, const Vector3 &p_vector);
Vector3 get_joint_limitation_right_axis_vector(int p_index, int p_joint) const;
void set_joint_limitation_rotation_offset(int p_index, int p_joint, const Quaternion &p_offset);
Quaternion get_joint_limitation_rotation_offset(int p_index, int p_joint) const;
// Helper.
Quaternion get_joint_limitation_space(int p_index, int p_joint, const Vector3 &p_forward) const;
~IterateIK3D();
};

72
scene/3d/jacobian_ik_3d.cpp Normal file
View file

@ -0,0 +1,72 @@
/**************************************************************************/
/* jacobian_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 "jacobian_ik_3d.h"
void JacobianIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
int joint_size = (int)p_setting->joints.size();
int chain_size = (int)p_setting->chain.size();
// Forwards.
for (int i = 0; i < joint_size; i++) {
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
int HEAD = i;
int TAIL = i + 1;
Vector3 current_head = p_setting->chain[HEAD];
Vector3 current_effector = p_setting->chain[chain_size - 1];
Vector3 head_to_effector = current_effector - current_head;
Vector3 effector_to_destination = p_destination - current_effector;
Vector3 axis = head_to_effector.cross(effector_to_destination);
if (Math::is_zero_approx(axis.length_squared())) {
continue;
}
Quaternion to_rot = Quaternion(axis.normalized(), axis.length() / MAX(CMP_EPSILON, head_to_effector.length()));
for (int j = TAIL; j < chain_size; j++) {
Vector3 to_tail = p_setting->chain[j] - current_head;
p_setting->update_chain_coordinate_fw(p_skeleton, j, current_head + to_rot.xform(to_tail));
int k = j - 1;
if (p_setting->joint_settings[k]->rotation_axis != ROTATION_AXIS_ALL) {
p_setting->update_chain_coordinate_fw(p_skeleton, j, p_setting->chain[k] + p_setting->joint_settings[k]->get_projected_rotation(solver_info->current_grest, p_setting->chain[j] - p_setting->chain[k]));
}
if (p_setting->joint_settings[k]->limitation.is_valid()) {
p_setting->update_chain_coordinate_fw(p_skeleton, j, p_setting->chain[k] + p_setting->joint_settings[k]->get_limited_rotation(solver_info->current_grest, p_setting->chain[j] - p_setting->chain[k], solver_info->forward_vector));
}
}
}
}

40
scene/3d/jacobian_ik_3d.h Normal file
View file

@ -0,0 +1,40 @@
/**************************************************************************/
/* jacobian_ik_3d.h */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#pragma once
#include "scene/3d/iterate_ik_3d.h"
class JacobianIK3D : public IterateIK3D {
GDCLASS(JacobianIK3D, IterateIK3D);
protected:
virtual void _solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) override;
};

8
scene/3d/look_at_modifier_3d.cpp
View file

@ -34,7 +34,7 @@ void LookAtModifier3D::_validate_property(PropertyInfo &p_property) const {
if (Engine::get_singleton()->is_editor_hint() && (p_property.name == "bone_name" || p_property.name == "origin_bone_name")) {
Skeleton3D *skeleton = get_skeleton();
if (skeleton) {
p_property.hint = PROPERTY_HINT_ENUM;
p_property.hint = PROPERTY_HINT_ENUM_SUGGESTION;
p_property.hint_string = skeleton->get_concatenated_bone_names();
} else {
p_property.hint = PROPERTY_HINT_NONE;
@ -457,7 +457,7 @@ void LookAtModifier3D::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::STRING, "bone_name", PROPERTY_HINT_ENUM_SUGGESTION, ""), "set_bone_name", "get_bone_name");
ADD_PROPERTY(PropertyInfo(Variant::INT, "bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_bone", "get_bone");
ADD_PROPERTY(PropertyInfo(Variant::INT, "forward_axis", PROPERTY_HINT_ENUM, "+X,-X,+Y,-Y,+Z,-Z"), "set_forward_axis", "get_forward_axis");
ADD_PROPERTY(PropertyInfo(Variant::INT, "forward_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_bone_axis()), "set_forward_axis", "get_forward_axis");
ADD_PROPERTY(PropertyInfo(Variant::INT, "primary_rotation_axis", PROPERTY_HINT_ENUM, "X,Y,Z"), "set_primary_rotation_axis", "get_primary_rotation_axis");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_secondary_rotation"), "set_use_secondary_rotation", "is_using_secondary_rotation");
@ -500,10 +500,6 @@ void LookAtModifier3D::_bind_methods() {
}
void LookAtModifier3D::_process_modification(double p_delta) {
if (!is_inside_tree()) {
return;
}
Skeleton3D *skeleton = get_skeleton();
if (!skeleton || bone < 0 || bone >= skeleton->get_bone_count()) {
return;

6
scene/3d/modifier_bone_target_3d.cpp
View file

@ -78,7 +78,7 @@ void ModifierBoneTarget3D::_validate_property(PropertyInfo &p_property) const {
if (p_property.name == "bone_name") {
Skeleton3D *skeleton = get_skeleton();
if (skeleton) {
p_property.hint = PROPERTY_HINT_ENUM;
p_property.hint = PROPERTY_HINT_ENUM_SUGGESTION;
p_property.hint_string = skeleton->get_concatenated_bone_names();
} else {
p_property.hint = PROPERTY_HINT_NONE;
@ -98,10 +98,6 @@ void ModifierBoneTarget3D::_bind_methods() {
}
void ModifierBoneTarget3D::_process_modification(double p_delta) {
if (!is_inside_tree()) {
return;
}
Skeleton3D *skeleton = get_skeleton();
if (!skeleton || bone < 0 || bone >= skeleton->get_bone_count()) {
return;

3
scene/3d/path_3d.h
View file

@ -63,9 +63,6 @@ public:
const Color &get_debug_custom_color() const;
void set_debug_custom_color(const Color &p_color);
bool get_debug_show() const;
void set_debug_show(bool p_show);
Ref<StandardMaterial3D> get_debug_material();
Path3D();

4
scene/3d/skeleton_3d.cpp
View file

@ -942,7 +942,7 @@ void Skeleton3D::_update_deferred(UpdateFlag p_update_flag) {
_notification(NOTIFICATION_UPDATE_SKELETON);
return;
}
#endif //TOOLS_ENABLED
#endif // TOOLS_ENABLED
if (update_flags == UPDATE_FLAG_NONE && !updating) {
notify_deferred_thread_group(NOTIFICATION_UPDATE_SKELETON); // It must never be called more than once in a single frame.
}
@ -1177,7 +1177,7 @@ void Skeleton3D::_process_modifiers() {
if (saving && !mod->is_processed_on_saving()) {
continue;
}
#endif //TOOLS_ENABLED
#endif // TOOLS_ENABLED
real_t influence = mod->get_influence();
if (influence < 1.0) {
LocalVector<Transform3D> old_poses;

2
scene/3d/skeleton_3d.h
View file

@ -67,7 +67,7 @@ class Skeleton3D : public Node3D {
#ifdef TOOLS_ENABLED
bool saving = false;
#endif //TOOLS_ENABLED
#endif // TOOLS_ENABLED
#if !defined(DISABLE_DEPRECATED) && !defined(PHYSICS_3D_DISABLED)
bool animate_physical_bones = true;

120
scene/3d/skeleton_modifier_3d.cpp
View file

@ -117,7 +117,7 @@ real_t SkeletonModifier3D::get_influence() const {
}
void SkeletonModifier3D::process_modification(double p_delta) {
if (!active) {
if (!is_inside_tree() || !active) {
return;
}
_process_modification(p_delta);
@ -175,6 +175,29 @@ void SkeletonModifier3D::_bind_methods() {
BIND_ENUM_CONSTANT(BONE_AXIS_MINUS_Y);
BIND_ENUM_CONSTANT(BONE_AXIS_PLUS_Z);
BIND_ENUM_CONSTANT(BONE_AXIS_MINUS_Z);
BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_X);
BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_X);
BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_Y);
BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_Y);
BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_Z);
BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_Z);
BIND_ENUM_CONSTANT(BONE_DIRECTION_FROM_PARENT);
BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_NONE);
BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_PLUS_X);
BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_MINUS_X);
BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_PLUS_Y);
BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_MINUS_Y);
BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_PLUS_Z);
BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_MINUS_Z);
BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_CUSTOM);
BIND_ENUM_CONSTANT(ROTATION_AXIS_X);
BIND_ENUM_CONSTANT(ROTATION_AXIS_Y);
BIND_ENUM_CONSTANT(ROTATION_AXIS_Z);
BIND_ENUM_CONSTANT(ROTATION_AXIS_ALL);
BIND_ENUM_CONSTANT(ROTATION_AXIS_CUSTOM);
}
Vector3 SkeletonModifier3D::get_vector_from_bone_axis(BoneAxis p_axis) {
@ -246,7 +269,7 @@ Quaternion SkeletonModifier3D::get_local_pose_rotation(Skeleton3D *p_skeleton, i
if (parent < 0) {
return p_global_pose_rotation;
}
return p_skeleton->get_bone_global_pose(parent).basis.orthonormalized().inverse() * p_global_pose_rotation;
return (p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion().inverse() * p_global_pose_rotation).normalized();
}
Quaternion SkeletonModifier3D::get_from_to_rotation(const Vector3 &p_from, const Vector3 &p_to, const Quaternion &p_prev_rot) {
@ -264,6 +287,47 @@ Quaternion SkeletonModifier3D::get_from_to_rotation(const Vector3 &p_from, const
return Quaternion(axis.normalized(), angle);
}
Quaternion SkeletonModifier3D::get_from_to_rotation_by_axis(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_axis) {
const double ALMOST_ONE = 1.0 - CMP_EPSILON;
double dot = p_from.dot(p_to);
if (dot > ALMOST_ONE) {
return Quaternion();
}
if (dot < -ALMOST_ONE) {
return Quaternion(p_axis, Math::PI);
}
double angle = p_from.angle_to(p_to);
Vector3 cross = p_from.cross(p_to);
if (std::signbit(cross.dot(p_axis))) {
angle = -angle;
}
return Quaternion(p_axis, angle);
}
Quaternion SkeletonModifier3D::get_swing(const Quaternion &p_rotation, const Vector3 &p_axis) {
if (p_axis.is_zero_approx()) {
return p_rotation;
}
Quaternion rot = p_rotation;
if (!rot.is_normalized()) {
rot.normalize();
}
Vector3 axis = p_axis.normalized();
const Vector3 v(rot.x, rot.y, rot.z);
const real_t proj_len = v.dot(axis);
const Vector3 twist_vec = axis * proj_len;
Quaternion twist(twist_vec.x, twist_vec.y, twist_vec.z, rot.w);
if (!twist.is_normalized()) {
if (Math::is_zero_approx(twist.length_squared())) {
return rot;
}
twist.normalize();
}
Quaternion swing = rot * twist.inverse();
swing.normalize();
return swing;
}
Vector3 SkeletonModifier3D::snap_vector_to_plane(const Vector3 &p_plane_normal, const Vector3 &p_vector) {
if (Math::is_zero_approx(p_plane_normal.length_squared())) {
return p_vector;
@ -274,5 +338,57 @@ Vector3 SkeletonModifier3D::snap_vector_to_plane(const Vector3 &p_plane_normal,
return normalized_vec.slide(normal) * length;
}
double SkeletonModifier3D::symmetrize_angle(double p_angle) {
double angle = Math::fposmod(p_angle, Math::TAU);
return angle > Math::PI ? angle - Math::TAU : angle;
}
double SkeletonModifier3D::get_roll_angle(const Quaternion &p_rotation, const Vector3 &p_roll_axis) {
// Ensure roll axis is normalized.
Vector3 roll_axis = p_roll_axis.normalized();
// Project the quaternion rotation onto the roll axis.
// This gives us the component of rotation around that axis.
double dot = p_rotation.x * roll_axis.x +
p_rotation.y * roll_axis.y +
p_rotation.z * roll_axis.z;
// Create a quaternion representing just the roll component.
Quaternion roll_component;
roll_component.x = roll_axis.x * dot;
roll_component.y = roll_axis.y * dot;
roll_component.z = roll_axis.z * dot;
roll_component.w = p_rotation.w;
// Normalize this component.
double length = roll_component.length();
if (length > CMP_EPSILON) {
roll_component = roll_component / length;
} else {
return 0.0;
}
// Extract the angle.
double angle = 2.0 * Math::acos(CLAMP(roll_component.w, -1.0, 1.0));
// Determine the sign.
double direction = (roll_component.x * roll_axis.x + roll_component.y * roll_axis.y + roll_component.z * roll_axis.z > 0) ? 1.0 : -1.0;
return symmetrize_angle(angle * direction);
}
Vector3 SkeletonModifier3D::get_projected_normal(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_point) {
// Get nearest normal vector to p_point from the infinite line p_a to p_b.
const Vector3 dir = p_b - p_a;
const real_t denom = dir.length_squared();
if (Math::is_zero_approx(denom)) {
return Vector3();
}
const Vector3 w = p_point - p_a;
const real_t t = w.dot(dir) / denom;
const Vector3 h = p_a + dir * t;
return (p_point - h).normalized();
}
SkeletonModifier3D::SkeletonModifier3D() {
}

46
scene/3d/skeleton_modifier_3d.h
View file

@ -40,6 +40,7 @@ class SkeletonModifier3D : public Node3D {
void rebind();
public:
// For the case to indicate bone axis on basis without custom vector.
enum BoneAxis {
BONE_AXIS_PLUS_X,
BONE_AXIS_MINUS_X,
@ -48,6 +49,42 @@ public:
BONE_AXIS_PLUS_Z,
BONE_AXIS_MINUS_Z,
};
static String get_hint_bone_axis() { return "+X,-X,+Y,-Y,+Z,-Z"; }
// For the case to indicate Head-Tail of the bone.
enum BoneDirection {
BONE_DIRECTION_PLUS_X,
BONE_DIRECTION_MINUS_X,
BONE_DIRECTION_PLUS_Y,
BONE_DIRECTION_MINUS_Y,
BONE_DIRECTION_PLUS_Z,
BONE_DIRECTION_MINUS_Z,
BONE_DIRECTION_FROM_PARENT,
};
static String get_hint_bone_direction() { return "+X,-X,+Y,-Y,+Z,-Z,FromParent"; }
// For the case to define secondary axis of the bone local space.
enum SecondaryDirection {
SECONDARY_DIRECTION_NONE,
SECONDARY_DIRECTION_PLUS_X,
SECONDARY_DIRECTION_MINUS_X,
SECONDARY_DIRECTION_PLUS_Y,
SECONDARY_DIRECTION_MINUS_Y,
SECONDARY_DIRECTION_PLUS_Z,
SECONDARY_DIRECTION_MINUS_Z,
SECONDARY_DIRECTION_CUSTOM,
};
static String get_hint_secondary_direction() { return "None,+X,-X,+Y,-Y,+Z,-Z,Custom"; }
// For the case to define rotation direction without identification plus/minus.
enum RotationAxis {
ROTATION_AXIS_X,
ROTATION_AXIS_Y,
ROTATION_AXIS_Z,
ROTATION_AXIS_ALL,
ROTATION_AXIS_CUSTOM,
};
static String get_hint_rotation_axis() { return "X,Y,Z,All,Custom"; }
protected:
bool active = true;
@ -96,10 +133,16 @@ public:
static Vector3 get_vector_from_axis(Vector3::Axis p_axis);
static Vector3::Axis get_axis_from_bone_axis(BoneAxis p_axis);
// 3D math.
static Vector3 limit_length(const Vector3 &p_origin, const Vector3 &p_destination, float p_length);
static Quaternion get_local_pose_rotation(Skeleton3D *p_skeleton, int p_bone, const Quaternion &p_global_pose_rotation);
static Quaternion get_from_to_rotation(const Vector3 &p_from, const Vector3 &p_to, const Quaternion &p_prev_rot);
static Quaternion get_from_to_rotation_by_axis(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_axis);
static Quaternion get_swing(const Quaternion &p_rotation, const Vector3 &p_axis);
static Vector3 snap_vector_to_plane(const Vector3 &p_plane_normal, const Vector3 &p_vector);
static double symmetrize_angle(double p_angle);
static double get_roll_angle(const Quaternion &p_rotation, const Vector3 &p_roll_axis);
static Vector3 get_projected_normal(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_point);
#ifdef TOOLS_ENABLED
virtual bool is_processed_on_saving() const { return false; }
@ -109,3 +152,6 @@ public:
};
VARIANT_ENUM_CAST(SkeletonModifier3D::BoneAxis);
VARIANT_ENUM_CAST(SkeletonModifier3D::BoneDirection);
VARIANT_ENUM_CAST(SkeletonModifier3D::SecondaryDirection);
VARIANT_ENUM_CAST(SkeletonModifier3D::RotationAxis);

435
scene/3d/spline_ik_3d.cpp Normal file
View file

@ -0,0 +1,435 @@
/**************************************************************************/
/* spline_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 "spline_ik_3d.h"
bool SplineIK3D::_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 == "path_3d") {
set_path_3d(which, p_value);
} else if (what == "tilt_enabled") {
set_tilt_enabled(which, p_value);
} else if (what == "tilt_fade_in") {
set_tilt_fade_in(which, p_value);
} else if (what == "tilt_fade_out") {
set_tilt_fade_out(which, p_value);
} else {
return false;
}
}
return true;
}
bool SplineIK3D::_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 == "path_3d") {
r_ret = get_path_3d(which);
} else if (what == "tilt_enabled") {
r_ret = is_tilt_enabled(which);
} else if (what == "tilt_fade_in") {
r_ret = get_tilt_fade_in(which);
} else if (what == "tilt_fade_out") {
r_ret = get_tilt_fade_out(which);
} else {
return false;
}
}
return true;
}
void SplineIK3D::_get_property_list(List<PropertyInfo> *p_list) const {
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 + "path_3d", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Path3D"));
props.push_back(PropertyInfo(Variant::BOOL, path + "tilt_enabled"));
props.push_back(PropertyInfo(Variant::INT, path + "tilt_fade_in", PROPERTY_HINT_RANGE, "-1,100,1,or_greater"));
props.push_back(PropertyInfo(Variant::INT, path + "tilt_fade_out", PROPERTY_HINT_RANGE, "-1,100,1,or_greater"));
}
for (PropertyInfo &p : props) {
_validate_dynamic_prop(p);
p_list->push_back(p);
}
ChainIK3D::get_property_list(p_list);
}
void SplineIK3D::_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();
if (split[2].begins_with("tilt_") && get_path_3d(which).is_empty()) {
p_property.usage = PROPERTY_USAGE_NONE;
} else if (split[2].begins_with("tilt_fade_") && !is_tilt_enabled(which)) {
p_property.usage = PROPERTY_USAGE_NONE;
}
}
}
PackedStringArray SplineIK3D::get_configuration_warnings() const {
PackedStringArray warnings = SkeletonModifier3D::get_configuration_warnings();
for (uint32_t i = 0; i < sp_settings.size(); i++) {
if (sp_settings[i]->path_3d.is_empty()) {
warnings.push_back(RTR("Detecting settings with no Path3D set! SplineIK3D must have a Path3D to work."));
break;
}
}
return warnings;
}
// Setting.
void SplineIK3D::set_path_3d(int p_index, const NodePath &p_path_3d) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
sp_settings[p_index]->path_3d = p_path_3d;
notify_property_list_changed();
update_configuration_warnings();
}
NodePath SplineIK3D::get_path_3d(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
return sp_settings[p_index]->path_3d;
}
void SplineIK3D::set_tilt_enabled(int p_index, bool p_enabled) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
sp_settings[p_index]->tilt_enabled = p_enabled;
notify_property_list_changed();
}
bool SplineIK3D::is_tilt_enabled(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
return sp_settings[p_index]->tilt_enabled;
}
void SplineIK3D::set_tilt_fade_in(int p_index, int p_size) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
sp_settings[p_index]->tilt_fade_in = p_size;
}
int SplineIK3D::get_tilt_fade_in(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
return sp_settings[p_index]->tilt_fade_in;
}
void SplineIK3D::set_tilt_fade_out(int p_index, int p_size) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
sp_settings[p_index]->tilt_fade_out = p_size;
}
int SplineIK3D::get_tilt_fade_out(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
return sp_settings[p_index]->tilt_fade_out;
}
// Individual joints.
void SplineIK3D::_set_joint_count(int p_index, int p_count) {
LocalVector<double> &twists = sp_settings[p_index]->twists;
twists.resize(p_count);
LocalVector<double> &accum = sp_settings[p_index]->chain_length_accum;
accum.resize(p_count);
}
void SplineIK3D::_bind_methods() {
// Setting.
ClassDB::bind_method(D_METHOD("set_path_3d", "index", "path_3d"), &SplineIK3D::set_path_3d);
ClassDB::bind_method(D_METHOD("get_path_3d", "index"), &SplineIK3D::get_path_3d);
ClassDB::bind_method(D_METHOD("set_tilt_enabled", "index", "enabled"), &SplineIK3D::set_tilt_enabled);
ClassDB::bind_method(D_METHOD("is_tilt_enabled", "index"), &SplineIK3D::is_tilt_enabled);
ClassDB::bind_method(D_METHOD("set_tilt_fade_in", "index", "size"), &SplineIK3D::set_tilt_fade_in);
ClassDB::bind_method(D_METHOD("get_tilt_fade_in", "index"), &SplineIK3D::get_tilt_fade_in);
ClassDB::bind_method(D_METHOD("set_tilt_fade_out", "index", "size"), &SplineIK3D::set_tilt_fade_out);
ClassDB::bind_method(D_METHOD("get_tilt_fade_out", "index"), &SplineIK3D::get_tilt_fade_out);
ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
}
void SplineIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
SplineIK3DSetting *setting = sp_settings[p_index];
cached_space = p_skeleton->get_global_transform_interpolated();
if (!setting->simulation_dirty) {
return;
}
for (uint32_t i = 0; i < setting->solver_info_list.size(); i++) {
if (setting->solver_info_list[i]) {
memdelete(setting->solver_info_list[i]);
}
}
setting->solver_info_list.clear();
setting->solver_info_list.resize_initialized(setting->joints.size());
setting->chain.clear();
bool extend_end_bone = setting->extend_end_bone && setting->end_bone_length > 0;
double accum = 0.0;
for (uint32_t i = 0; i < setting->joints.size(); i++) {
setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).origin);
bool last = i == setting->joints.size() - 1;
if (last && extend_end_bone && setting->end_bone_length > 0) {
Vector3 axis = get_bone_axis(setting->end_bone.bone, setting->end_bone_direction);
if (axis.is_zero_approx()) {
setting->chain_length_accum[i] = accum;
continue;
}
setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
setting->solver_info_list[i]->forward_vector = axis.normalized();
setting->solver_info_list[i]->length = setting->end_bone_length;
setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).xform(axis * setting->end_bone_length));
} else if (!last) {
Vector3 axis = p_skeleton->get_bone_rest(setting->joints[i + 1].bone).origin;
if (axis.is_zero_approx()) {
setting->chain_length_accum[i] = accum;
continue; // Means always we need to check solver info, but `!solver_info` means that the bone is zero length, so IK should skip it in the all process.
}
setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
setting->solver_info_list[i]->forward_vector = axis.normalized();
setting->solver_info_list[i]->length = axis.length();
}
if (setting->solver_info_list[i]) {
accum += setting->solver_info_list[i]->length;
}
setting->chain_length_accum[i] = accum;
}
setting->init_current_joint_rotations(p_skeleton);
setting->simulation_dirty = false;
}
void SplineIK3D::_make_simulation_dirty(int p_index) {
SplineIK3DSetting *setting = sp_settings[p_index];
if (!setting) {
return;
}
setting->simulation_dirty = true;
}
void SplineIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
for (uint32_t i = 0; i < settings.size(); i++) {
_init_joints(p_skeleton, i);
if (sp_settings[i]->joints.is_empty()) {
continue; // Abort.
}
Path3D *path_3d = Object::cast_to<Path3D>(get_node_or_null(sp_settings[i]->path_3d));
if (!path_3d) {
continue; // Abort.
}
Ref<Curve3D> curve = path_3d->get_curve();
if (curve.is_null() || curve->get_point_count() == 0) {
continue; // Abort.
}
sp_settings[i]->cache_current_joint_rotations(p_skeleton); // Iterate over first to detect parent (outside of the chain) bone pose changes.
_process_joints(p_delta, p_skeleton, sp_settings[i], curve, cached_space.affine_inverse() * path_3d->get_global_transform_interpolated());
}
}
void SplineIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, SplineIK3DSetting *p_setting, Ref<Curve3D> p_curve, const Transform3D &p_curve_space) {
if (p_setting->solver_info_list.is_empty()) {
return;
}
uint32_t joint_count = p_setting->joints.size();
uint32_t joint_last = joint_count - 1;
double path_length = p_curve->get_baked_length();
PackedVector3Array points = p_curve->get_baked_points();
Vector<real_t> tilts = p_curve->get_baked_tilts();
Vector<real_t> dists = p_curve->get_baked_dist_cache();
uint32_t point_count = points.size();
uint32_t point_last = point_count - 1;
// Make straight segment from root joint to start point.
Vector3 start_point = p_curve_space.xform(points[0]);
Vector3 start_vector = start_point - p_skeleton->get_bone_global_pose(p_setting->joints[0].bone).origin;
double start_dist = start_vector.length();
// Find first joint on the path.
uint32_t chain_path_start = 0;
while (chain_path_start < joint_count) {
if (p_setting->chain_length_accum[chain_path_start] >= start_dist) {
break;
}
chain_path_start++;
}
chain_path_start = (uint32_t)CLAMP((int)chain_path_start, 0, (int)joint_last);
// For tilt fade-in, get bones length not on the path as denominator.
double fade_in_denom = 0.0;
int denom_start = p_setting->tilt_fade_in > 0 ? CLAMP(p_setting->tilt_fade_in - 1, (int)chain_path_start, (int)joint_count) : -1;
int denom_start_to = denom_start - p_setting->tilt_fade_in;
if (denom_start >= 0) {
for (int i = denom_start; i > denom_start_to; i--) {
if (i < 0) {
break;
}
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
fade_in_denom += solver_info->length;
}
}
// Prepare for fade-out.
uint32_t ended = 0;
Vector3 end_point = p_curve_space.xform(points[point_last]);
Vector3 end_vector;
double end_to_end_length = 0.0;
double fade_out_denom = 0.0;
uint32_t last_nearest = 0;
uint32_t last_nearest_next = 0;
double last_interpolate = 0.0;
for (uint32_t i = 0; i < p_setting->solver_info_list.size(); i++) {
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
uint32_t HEAD = i;
uint32_t TAIL = i + 1;
bool is_fitting_first = HEAD == chain_path_start;
// Special case for out of path joints.
if (point_count == 1 || HEAD <= chain_path_start) {
// Set twist only for first fitting joint.
if (!is_fitting_first) {
p_setting->update_chain_coordinate(p_skeleton, TAIL, limit_length(p_setting->chain[HEAD], p_setting->chain[HEAD] + start_vector, solver_info->length));
}
if (p_setting->tilt_enabled) {
if (p_setting->tilt_fade_in < 0) {
p_setting->twists[HEAD] = 0.0;
} else if (p_setting->tilt_fade_in == 0) {
p_setting->twists[HEAD] = tilts[0];
} else {
// Decreases monotonically in a straight line, fetch the distance.
double fade_in_dumping = CLAMP((double)(p_setting->chain[HEAD].distance_to(start_point) / fade_in_denom), 0.0, 1.0);
p_setting->twists[HEAD] = Math::lerp((double)tilts[0], 0.0, fade_in_dumping);
}
}
if (!is_fitting_first) {
continue;
}
} else if (ended > 0) {
p_setting->update_chain_coordinate(p_skeleton, TAIL, limit_length(p_setting->chain[HEAD], p_setting->chain[HEAD] + end_vector, solver_info->length));
if (p_setting->tilt_enabled) {
if (p_setting->tilt_fade_out < 0) {
p_setting->twists[HEAD] = 0.0;
} else if (p_setting->tilt_fade_out == 0) {
p_setting->twists[HEAD] = tilts[point_last];
} else {
// Increases monotonically in a bended line, accumulate the distances.
if (ended == 1) {
end_to_end_length = p_setting->chain[TAIL].distance_to(end_point);
} else {
end_to_end_length += solver_info->length;
}
double fade_out_dumping = CLAMP(end_to_end_length / fade_out_denom, 0.0, 1.0);
p_setting->twists[HEAD] = Math::lerp(ended == 1 ? Math::lerp((double)tilts[last_nearest], (double)tilts[last_nearest_next], last_interpolate) : (double)tilts[point_last], 0.0, fade_out_dumping);
ended = 2;
}
}
continue;
}
// General case.
double lsq = solver_info->length * solver_info->length;
Vector3 head_in_chain_space = p_curve_space.xform_inv(p_setting->chain[HEAD]);
double interpolate = 0.0;
uint32_t nearest = p_setting->find_nearest_point(head_in_chain_space, lsq, points, p_curve->is_closed(), last_nearest, &interpolate);
if (nearest >= point_count) {
if (HEAD == 0) {
nearest = point_count - 2;
interpolate = 1.0;
} else {
Vector3 chain_end = (p_setting->chain[HEAD] - p_setting->chain[HEAD - 1]).normalized();
Vector3 path_end = (p_curve_space.xform(points[point_last]) - p_setting->chain[HEAD]).normalized();
double rest_path_length = path_length - Math::lerp((double)dists[last_nearest], (double)dists[last_nearest_next], last_interpolate);
interpolate = CLAMP(rest_path_length / solver_info->length, 0.0, 1.0); // End vector should be defined only one end bone to make neat interpolating.
end_vector = chain_end.lerp(path_end, interpolate);
int denom_end = p_setting->tilt_fade_out > 0 ? CLAMP((int)joint_last - p_setting->tilt_fade_out, 0, (int)last_nearest) : -1;
int denom_end_to = denom_end + p_setting->tilt_fade_out;
if (denom_end >= 0) {
for (int e = denom_end; e < denom_end_to; e++) {
if (e >= (int)joint_count) {
break;
}
IKModifier3DSolverInfo *end_solver_info = p_setting->solver_info_list[e];
if (!end_solver_info || Math::is_zero_approx(end_solver_info->length)) {
continue;
}
fade_out_denom += end_solver_info->length;
}
}
ended = 1;
i--; // Will be processed above special case.
continue;
}
}
uint32_t nearest_next = p_curve->is_closed() ? Math::posmod(nearest + 1, point_count) : CLAMP(nearest, (uint32_t)0, point_last);
p_setting->update_chain_coordinate(p_skeleton, TAIL, limit_length(p_setting->chain[HEAD], p_curve_space.xform(points[nearest].lerp(points[nearest_next], interpolate)), solver_info->length));
if (!is_fitting_first) {
p_setting->twists[HEAD] = Math::lerp((double)tilts[last_nearest], (double)tilts[last_nearest_next], last_interpolate);
}
last_nearest = nearest;
last_nearest_next = nearest_next;
last_interpolate = interpolate;
}
// Update virtual bone rest/poses.
p_setting->cache_current_joint_rotations(p_skeleton, p_setting->tilt_enabled); // Pass p_setting->tilt_enabled to skip unneeded rotate process.
// Apply the virtual bone rest/poses to the actual bones.
for (uint32_t i = 0; i < p_setting->solver_info_list.size(); i++) {
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
p_skeleton->set_bone_pose_rotation(p_setting->joints[i].bone, solver_info->current_lpose);
}
}
SplineIK3D::~SplineIK3D() {
clear_settings();
}

187
scene/3d/spline_ik_3d.h Normal file
View file

@ -0,0 +1,187 @@
/**************************************************************************/
/* spline_ik_3d.h */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#pragma once
#include "scene/3d/chain_ik_3d.h"
#include "scene/3d/path_3d.h"
class SplineIK3D : public ChainIK3D {
GDCLASS(SplineIK3D, ChainIK3D);
public:
struct SplineIK3DSetting : public ChainIK3DSetting {
NodePath path_3d;
bool tilt_enabled = true;
int tilt_fade_in = 1;
int tilt_fade_out = 1;
LocalVector<double> chain_length_accum;
LocalVector<double> twists;
// Find the nearest point.
int32_t find_nearest_point(const Vector3 &p_origin, double p_length_sq, const PackedVector3Array &p_points, bool p_is_path_closed, uint32_t p_first, double *r_ret) {
ERR_FAIL_COND_V(p_first >= p_points.size(), p_points.size() - 1);
uint32_t i = p_first;
if (p_is_path_closed) {
while (true) {
if (p_origin.distance_squared_to(p_points[i]) >= p_length_sq) {
break;
}
i++;
i = Math::posmod(i, p_points.size());
if (i == p_first) {
i = p_points.size();
break; // Can't found, use last.
}
}
} else {
while (i < p_points.size()) {
if (p_origin.distance_squared_to(p_points[i]) >= p_length_sq) {
break;
}
i++;
}
}
if (i == 0) {
*r_ret = 0.0;
return p_first;
} else if (i >= p_points.size()) {
*r_ret = 1.0;
return i;
}
*r_ret = Math::inverse_lerp((double)p_origin.distance_squared_to(p_points[i - 1]), (double)p_origin.distance_squared_to(p_points[i]), p_length_sq);
return i - 1;
}
// Make rotation as bone pose from chain coordinates.
void cache_current_joint_rotations(Skeleton3D *p_skeleton, bool p_use_tilt = false) {
Transform3D parent_gpose_tr;
int parent = p_skeleton->get_bone_parent(root_bone.bone);
if (parent >= 0) {
parent_gpose_tr = p_skeleton->get_bone_global_pose(parent);
}
Quaternion parent_gpose = parent_gpose_tr.basis.get_rotation_quaternion();
if (p_use_tilt) {
double parent_twist = 0.0;
for (uint32_t i = 0; i < joints.size(); i++) {
int HEAD = i;
IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
if (!solver_info) {
continue;
}
solver_info->current_lrest = p_skeleton->get_bone_pose(joints[HEAD].bone).basis.get_rotation_quaternion();
solver_info->current_grest = parent_gpose * solver_info->current_lrest;
solver_info->current_grest.normalize();
Vector3 from = solver_info->forward_vector;
Vector3 to = solver_info->current_grest.xform_inv(solver_info->current_vector).normalized();
Basis b = get_swing(Quaternion(from, to), from);
b.rotate_local(from, twists[HEAD] - parent_twist);
parent_twist = twists[HEAD];
solver_info->current_lpose = solver_info->current_lrest * b.get_rotation_quaternion();
solver_info->current_gpose = parent_gpose * solver_info->current_lpose;
solver_info->current_gpose.normalize();
parent_gpose = solver_info->current_gpose;
}
} else {
for (uint32_t i = 0; i < joints.size(); i++) {
int HEAD = i;
IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
if (!solver_info) {
continue;
}
solver_info->current_lrest = p_skeleton->get_bone_pose(joints[HEAD].bone).basis.get_rotation_quaternion();
solver_info->current_grest = parent_gpose * solver_info->current_lrest;
solver_info->current_grest.normalize();
Vector3 from = solver_info->forward_vector;
Vector3 to = solver_info->current_grest.xform_inv(solver_info->current_vector).normalized();
solver_info->current_lpose = solver_info->current_lrest * get_swing(Quaternion(from, to), from);
solver_info->current_gpose = parent_gpose * solver_info->current_lpose;
solver_info->current_gpose.normalize();
parent_gpose = solver_info->current_gpose;
}
}
// To update positions in preprocess of _process_joints().
cache_current_vectors(p_skeleton);
}
};
protected:
LocalVector<SplineIK3DSetting *> sp_settings; // For caching.
bool _get(const StringName &p_path, Variant &r_ret) const;
bool _set(const StringName &p_path, const Variant &p_value);
void _get_property_list(List<PropertyInfo> *p_list) const;
void _validate_dynamic_prop(PropertyInfo &p_property) const;
static void _bind_methods();
virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override;
virtual void _make_simulation_dirty(int p_index) override;
virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override;
void _process_joints(double p_delta, Skeleton3D *p_skeleton, SplineIK3DSetting *p_setting, Ref<Curve3D> p_curve, const Transform3D &p_curve_space);
virtual void _set_joint_count(int p_index, int p_count) override;
public:
virtual PackedStringArray get_configuration_warnings() const override;
virtual void set_setting_count(int p_count) override {
_set_setting_count<SplineIK3DSetting>(p_count);
sp_settings = _cast_settings<SplineIK3DSetting>();
chain_settings = _cast_settings<ChainIK3DSetting>(); // Don't forget to sync super class settings.
}
virtual void clear_settings() override {
_set_setting_count<SplineIK3DSetting>(0);
sp_settings.clear();
chain_settings.clear(); // Don't forget to sync super class settings.
}
// Setting.
void set_path_3d(int p_index, const NodePath &p_path_3d);
NodePath get_path_3d(int p_index) const;
void set_tilt_enabled(int p_index, bool p_enabled);
bool is_tilt_enabled(int p_index) const;
void set_tilt_fade_in(int p_index, int p_size);
int get_tilt_fade_in(int p_index) const;
void set_tilt_fade_out(int p_index, int p_size);
int get_tilt_fade_out(int p_index) const;
// Helper.
double get_bezier_arc_length();
~SplineIK3D();
};

89
scene/3d/spring_bone_simulator_3d.compat.inc Normal file
View file

@ -0,0 +1,89 @@
/**************************************************************************/
/* spring_bone_simulator_3d.compat.inc */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#ifndef DISABLE_DEPRECATED
namespace compat::SpringBoneSimulator3D {
enum BoneDirection : int {
BONE_DIRECTION_PLUS_X = SkeletonModifier3D::BONE_DIRECTION_PLUS_X,
BONE_DIRECTION_MINUS_X = SkeletonModifier3D::BONE_DIRECTION_MINUS_X,
BONE_DIRECTION_PLUS_Y = SkeletonModifier3D::BONE_DIRECTION_PLUS_Y,
BONE_DIRECTION_MINUS_Y = SkeletonModifier3D::BONE_DIRECTION_MINUS_Y,
BONE_DIRECTION_PLUS_Z = SkeletonModifier3D::BONE_DIRECTION_PLUS_Z,
BONE_DIRECTION_MINUS_Z = SkeletonModifier3D::BONE_DIRECTION_MINUS_Z,
BONE_DIRECTION_FROM_PARENT = SkeletonModifier3D::BONE_DIRECTION_FROM_PARENT,
};
enum RotationAxis : int {
ROTATION_AXIS_X = SkeletonModifier3D::ROTATION_AXIS_X,
ROTATION_AXIS_Y = SkeletonModifier3D::ROTATION_AXIS_Y,
ROTATION_AXIS_Z = SkeletonModifier3D::ROTATION_AXIS_Z,
ROTATION_AXIS_ALL = SkeletonModifier3D::ROTATION_AXIS_ALL,
ROTATION_AXIS_CUSTOM = SkeletonModifier3D::ROTATION_AXIS_CUSTOM,
};
} //namespace compat::SpringBoneSimulator3D
VARIANT_ENUM_CAST(compat::SpringBoneSimulator3D::BoneDirection);
VARIANT_ENUM_CAST(compat::SpringBoneSimulator3D::RotationAxis);
compat::SpringBoneSimulator3D::BoneDirection SpringBoneSimulator3D::_get_end_bone_direction_bind_compat_110120(int p_index) const {
return static_cast<compat::SpringBoneSimulator3D::BoneDirection>((int)get_end_bone_direction(p_index));
}
void SpringBoneSimulator3D::_set_end_bone_direction_bind_compat_110120(int p_index, compat::SpringBoneSimulator3D::BoneDirection p_bone_direction) {
set_end_bone_direction(p_index, static_cast<SkeletonModifier3D::BoneDirection>((int)p_bone_direction));
}
compat::SpringBoneSimulator3D::RotationAxis SpringBoneSimulator3D::_get_rotation_axis_bind_compat_110120(int p_index) const {
return static_cast<compat::SpringBoneSimulator3D::RotationAxis>((int)get_rotation_axis(p_index));
}
void SpringBoneSimulator3D::_set_rotation_axis_bind_compat_110120(int p_index, compat::SpringBoneSimulator3D::RotationAxis p_axis) {
set_rotation_axis(p_index, static_cast<SkeletonModifier3D::RotationAxis>((int)p_axis));
}
compat::SpringBoneSimulator3D::RotationAxis SpringBoneSimulator3D::_get_joint_rotation_axis_bind_compat_110120(int p_index, int p_joint) const {
return static_cast<compat::SpringBoneSimulator3D::RotationAxis>((int)get_joint_rotation_axis(p_index, p_joint));
}
void SpringBoneSimulator3D::_set_joint_rotation_axis_bind_compat_110120(int p_index, int p_joint, compat::SpringBoneSimulator3D::RotationAxis p_axis) {
set_joint_rotation_axis(p_index, p_joint, static_cast<SkeletonModifier3D::RotationAxis>((int)p_axis));
}
void SpringBoneSimulator3D::_bind_compatibility_methods() {
ClassDB::bind_compatibility_method(D_METHOD("get_end_bone_direction", "index"), &SpringBoneSimulator3D::_get_end_bone_direction_bind_compat_110120);
ClassDB::bind_compatibility_method(D_METHOD("set_end_bone_direction", "index", "bone_direction"), &SpringBoneSimulator3D::_set_end_bone_direction_bind_compat_110120);
ClassDB::bind_compatibility_method(D_METHOD("get_rotation_axis", "index"), &SpringBoneSimulator3D::_get_rotation_axis_bind_compat_110120);
ClassDB::bind_compatibility_method(D_METHOD("set_rotation_axis", "index", "axis"), &SpringBoneSimulator3D::_set_rotation_axis_bind_compat_110120);
ClassDB::bind_compatibility_method(D_METHOD("get_joint_rotation_axis", "index", "joint"), &SpringBoneSimulator3D::_get_joint_rotation_axis_bind_compat_110120);
ClassDB::bind_compatibility_method(D_METHOD("set_joint_rotation_axis", "index", "joint", "axis"), &SpringBoneSimulator3D::_set_joint_rotation_axis_bind_compat_110120);
}
#endif // DISABLE_DEPRECATED

53
scene/3d/spring_bone_simulator_3d.cpp
View file

@ -29,6 +29,7 @@
/**************************************************************************/
#include "spring_bone_simulator_3d.h"
#include "spring_bone_simulator_3d.compat.inc"
#include "scene/3d/spring_bone_collision_3d.h"
@ -298,14 +299,14 @@ void SpringBoneSimulator3D::_get_property_list(List<PropertyInfo> *p_list) const
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 + "extend_end_bone"));
props.push_back(PropertyInfo(Variant::INT, path + "end_bone/direction", PROPERTY_HINT_ENUM, "+X,-X,+Y,-Y,+Z,-Z,FromParent"));
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"));
props.push_back(PropertyInfo(Variant::INT, path + "center_from", PROPERTY_HINT_ENUM, "WorldOrigin,Node,Bone"));
props.push_back(PropertyInfo(Variant::NODE_PATH, path + "center_node"));
props.push_back(PropertyInfo(Variant::STRING, path + "center_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
props.push_back(PropertyInfo(Variant::INT, path + "center_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
props.push_back(PropertyInfo(Variant::BOOL, path + "individual_config"));
props.push_back(PropertyInfo(Variant::INT, path + "rotation_axis", PROPERTY_HINT_ENUM, "X,Y,Z,All,Custom"));
props.push_back(PropertyInfo(Variant::INT, path + "rotation_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_rotation_axis()));
props.push_back(PropertyInfo(Variant::VECTOR3, path + "rotation_axis_vector"));
props.push_back(PropertyInfo(Variant::FLOAT, path + "radius/value", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"));
props.push_back(PropertyInfo(Variant::OBJECT, path + "radius/damping_curve", PROPERTY_HINT_RESOURCE_TYPE, "Curve"));
@ -319,9 +320,9 @@ void SpringBoneSimulator3D::_get_property_list(List<PropertyInfo> *p_list) const
props.push_back(PropertyInfo(Variant::INT, path + "joint_count", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_ARRAY, "Joints," + path + "joints/,static,const"));
for (uint32_t j = 0; j < settings[i]->joints.size(); j++) {
String joint_path = path + "joints/" + itos(j) + "/";
props.push_back(PropertyInfo(Variant::STRING, joint_path + "bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint, PROPERTY_USAGE_EDITOR | PROPERTY_USAGE_READ_ONLY | PROPERTY_USAGE_STORAGE));
props.push_back(PropertyInfo(Variant::STRING, joint_path + "bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint, PROPERTY_USAGE_EDITOR | PROPERTY_USAGE_READ_ONLY));
props.push_back(PropertyInfo(Variant::INT, joint_path + "bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_READ_ONLY));
props.push_back(PropertyInfo(Variant::INT, joint_path + "rotation_axis", PROPERTY_HINT_ENUM, "X,Y,Z,All,Custom"));
props.push_back(PropertyInfo(Variant::INT, joint_path + "rotation_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_rotation_axis()));
props.push_back(PropertyInfo(Variant::VECTOR3, joint_path + "rotation_axis_vector"));
props.push_back(PropertyInfo(Variant::FLOAT, joint_path + "radius", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"));
props.push_back(PropertyInfo(Variant::FLOAT, joint_path + "stiffness", PROPERTY_HINT_RANGE, "0,4,0.01,or_greater"));
@ -354,7 +355,12 @@ void SpringBoneSimulator3D::_validate_dynamic_prop(PropertyInfo &p_property) con
int which = split[1].to_int();
// Extended end bone option.
if (split[2] == "end_bone" && !is_end_bone_extended(which) && split.size() > 3) {
bool force_hide = false;
if (split[2] == "extend_end_bone" && 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;
}
@ -502,6 +508,7 @@ void SpringBoneSimulator3D::set_end_bone(int p_index, int p_bone) {
if (changed) {
_update_joint_array(p_index);
}
notify_property_list_changed();
}
int SpringBoneSimulator3D::get_end_bone(int p_index) const {
@ -527,7 +534,7 @@ void SpringBoneSimulator3D::set_end_bone_direction(int p_index, BoneDirection p_
_make_joints_dirty(p_index);
}
SpringBoneSimulator3D::BoneDirection SpringBoneSimulator3D::get_end_bone_direction(int p_index) const {
SkeletonModifier3D::BoneDirection SpringBoneSimulator3D::get_end_bone_direction(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), BONE_DIRECTION_FROM_PARENT);
return settings[p_index]->end_bone_direction;
}
@ -784,7 +791,7 @@ void SpringBoneSimulator3D::set_rotation_axis(int p_index, RotationAxis p_axis)
notify_property_list_changed();
}
SpringBoneSimulator3D::RotationAxis SpringBoneSimulator3D::get_rotation_axis(int p_index) const {
SkeletonModifier3D::RotationAxis SpringBoneSimulator3D::get_rotation_axis(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), ROTATION_AXIS_ALL);
return settings[p_index]->rotation_axis;
}
@ -828,6 +835,7 @@ void SpringBoneSimulator3D::set_setting_count(int p_count) {
if (delta < 0) {
for (int i = delta; i < 0; i++) {
memdelete(settings[(int)settings.size() + i]);
settings[(int)settings.size() + i] = nullptr;
}
}
settings.resize(p_count);
@ -863,6 +871,7 @@ bool SpringBoneSimulator3D::is_config_individual(int p_index) const {
}
void SpringBoneSimulator3D::set_joint_bone_name(int p_index, int p_joint, const String &p_bone_name) {
// Exists only for indicate bone name on the inspector, no needs to make dirty joint array.
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
ERR_FAIL_INDEX(p_joint, (int)joints.size());
@ -1011,7 +1020,7 @@ void SpringBoneSimulator3D::set_joint_rotation_axis(int p_index, int p_joint, Ro
#endif // TOOLS_ENABLED
}
SpringBoneSimulator3D::RotationAxis SpringBoneSimulator3D::get_joint_rotation_axis(int p_index, int p_joint) const {
SkeletonModifier3D::RotationAxis SpringBoneSimulator3D::get_joint_rotation_axis(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), ROTATION_AXIS_ALL);
const LocalVector<SpringBone3DJointSetting *> &joints = settings[p_index]->joints;
ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), ROTATION_AXIS_ALL);
@ -1051,6 +1060,7 @@ void SpringBoneSimulator3D::set_joint_count(int p_index, int p_count) {
if (delta < 0) {
for (int i = delta; i < 0; i++) {
memdelete(joints[joints.size() + i]);
joints[joints.size() + i] = nullptr;
}
}
joints.resize(p_count);
@ -1315,23 +1325,9 @@ void SpringBoneSimulator3D::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "external_force", PROPERTY_HINT_RANGE, "-99999,99999,or_greater,or_less,hide_control,suffix:m/s"), "set_external_force", "get_external_force");
ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_X);
BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_X);
BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_Y);
BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_Y);
BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_Z);
BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_Z);
BIND_ENUM_CONSTANT(BONE_DIRECTION_FROM_PARENT);
BIND_ENUM_CONSTANT(CENTER_FROM_WORLD_ORIGIN);
BIND_ENUM_CONSTANT(CENTER_FROM_NODE);
BIND_ENUM_CONSTANT(CENTER_FROM_BONE);
BIND_ENUM_CONSTANT(ROTATION_AXIS_X);
BIND_ENUM_CONSTANT(ROTATION_AXIS_Y);
BIND_ENUM_CONSTANT(ROTATION_AXIS_Z);
BIND_ENUM_CONSTANT(ROTATION_AXIS_ALL);
BIND_ENUM_CONSTANT(ROTATION_AXIS_CUSTOM);
}
void SpringBoneSimulator3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) {
@ -1420,7 +1416,7 @@ void SpringBoneSimulator3D::_validate_rotation_axis(Skeleton3D *p_skeleton, int
}
fwd.normalize();
if (Math::is_equal_approx(Math::abs(rot.dot(fwd)), 1)) {
WARN_PRINT_ED("Setting: " + itos(p_index) + " Joint: " + itos(p_joint) + ": Rotation axis and forward vectors are colinear. This is not advised as it may cause unwanted rotation.");
WARN_PRINT_ED("Setting: " + itos(p_index) + " Joint: " + itos(p_joint) + ": Rotation axis and forward vector are colinear. This is not advised as it may cause unwanted rotation.");
}
}
@ -1492,9 +1488,6 @@ void SpringBoneSimulator3D::_find_collisions() {
}
void SpringBoneSimulator3D::_process_collisions() {
if (!is_inside_tree()) {
return;
}
for (const ObjectID &oid : collisions) {
Object *t_obj = ObjectDB::get_instance(oid);
if (!t_obj) {
@ -1535,7 +1528,7 @@ void SpringBoneSimulator3D::_update_joint_array(int p_index) {
if (!valid) {
set_joint_count(p_index, 0);
ERR_FAIL_EDMSG("End bone must be the same as or a child of root bone.");
ERR_FAIL_EDMSG("End bone must be the same as or a child of the root bone.");
}
LocalVector<int> new_joints;
@ -1619,10 +1612,6 @@ void SpringBoneSimulator3D::_set_active(bool p_active) {
}
void SpringBoneSimulator3D::_process_modification(double p_delta) {
if (!is_inside_tree()) {
return;
}
Skeleton3D *skeleton = get_skeleton();
if (!skeleton) {
return;
@ -1634,7 +1623,7 @@ void SpringBoneSimulator3D::_process_modification(double p_delta) {
if (saving) {
return; // Collision position has been reset but we don't want to process simulating on saving. Abort.
}
#endif //TOOLS_ENABLED
#endif // TOOLS_ENABLED
for (uint32_t i = 0; i < settings.size(); i++) {
_init_joints(skeleton, settings[i]);

40
scene/3d/spring_bone_simulator_3d.h
View file

@ -32,12 +32,19 @@
#include "scene/3d/skeleton_modifier_3d.h"
#ifndef DISABLE_DEPRECATED
namespace compat::SpringBoneSimulator3D {
enum BoneDirection : int;
enum RotationAxis : int;
} //namespace compat::SpringBoneSimulator3D
#endif
class SpringBoneSimulator3D : public SkeletonModifier3D {
GDCLASS(SpringBoneSimulator3D, SkeletonModifier3D);
#ifdef TOOLS_ENABLED
bool saving = false;
#endif //TOOLS_ENABLED
#endif // TOOLS_ENABLED
bool joints_dirty = false;
@ -48,30 +55,12 @@ class SpringBoneSimulator3D : public SkeletonModifier3D {
void _make_collisions_dirty();
public:
enum BoneDirection {
BONE_DIRECTION_PLUS_X,
BONE_DIRECTION_MINUS_X,
BONE_DIRECTION_PLUS_Y,
BONE_DIRECTION_MINUS_Y,
BONE_DIRECTION_PLUS_Z,
BONE_DIRECTION_MINUS_Z,
BONE_DIRECTION_FROM_PARENT,
};
enum CenterFrom {
CENTER_FROM_WORLD_ORIGIN,
CENTER_FROM_NODE,
CENTER_FROM_BONE,
};
enum RotationAxis {
ROTATION_AXIS_X,
ROTATION_AXIS_Y,
ROTATION_AXIS_Z,
ROTATION_AXIS_ALL,
ROTATION_AXIS_CUSTOM,
};
struct SpringBone3DVerletInfo {
Vector3 prev_tail;
Vector3 current_tail;
@ -198,6 +187,17 @@ protected:
void _validate_rotation_axes(Skeleton3D *p_skeleton) const;
void _validate_rotation_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const;
#ifndef DISABLE_DEPRECATED
compat::SpringBoneSimulator3D::BoneDirection _get_end_bone_direction_bind_compat_110120(int p_index) const;
void _set_end_bone_direction_bind_compat_110120(int p_index, compat::SpringBoneSimulator3D::BoneDirection p_bone_direction);
compat::SpringBoneSimulator3D::RotationAxis _get_rotation_axis_bind_compat_110120(int p_index) const;
void _set_rotation_axis_bind_compat_110120(int p_index, compat::SpringBoneSimulator3D::RotationAxis p_axis);
compat::SpringBoneSimulator3D::RotationAxis _get_joint_rotation_axis_bind_compat_110120(int p_index, int p_joint) const;
void _set_joint_rotation_axis_bind_compat_110120(int p_index, int p_joint, compat::SpringBoneSimulator3D::RotationAxis p_axis);
static void _bind_compatibility_methods();
#endif // DISABLE_DEPRECATED
public:
// Setting.
void set_root_bone_name(int p_index, const String &p_bone_name);
@ -314,6 +314,4 @@ public:
~SpringBoneSimulator3D();
};
VARIANT_ENUM_CAST(SpringBoneSimulator3D::BoneDirection);
VARIANT_ENUM_CAST(SpringBoneSimulator3D::CenterFrom);
VARIANT_ENUM_CAST(SpringBoneSimulator3D::RotationAxis);

778
scene/3d/two_bone_ik_3d.cpp Normal file
View file

@ -0,0 +1,778 @@
/**************************************************************************/
/* 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();
}

311
scene/3d/two_bone_ik_3d.h Normal file
View file

@ -0,0 +1,311 @@
/**************************************************************************/
/* two_bone_ik_3d.h */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#pragma once
#include "scene/3d/ik_modifier_3d.h"
class TwoBoneIK3D : public IKModifier3D {
GDCLASS(TwoBoneIK3D, IKModifier3D);
public:
struct TwoBoneIK3DSetting : public IKModifier3DSetting {
bool joints_dirty = false;
BoneJoint root_bone;
BoneJoint middle_bone;
BoneJoint end_bone;
// To make virtual end joint.
bool use_virtual_end = false;
bool extend_end_bone = false;
BoneDirection end_bone_direction = BONE_DIRECTION_FROM_PARENT;
float end_bone_length = 0.0;
NodePath pole_node;
SecondaryDirection pole_direction = SECONDARY_DIRECTION_NONE; // Sort to pole target plane.
Vector3 pole_direction_vector; // Custom vector.
NodePath target_node;
IKModifier3DSolverInfo *root_joint_solver_info = nullptr;
IKModifier3DSolverInfo *mid_joint_solver_info = nullptr;
Vector3 root_pos;
Vector3 mid_pos;
Vector3 end_pos;
// To process.
bool simulation_dirty = true;
double cached_length_sq = 0.0;
bool is_valid() const {
return root_joint_solver_info && mid_joint_solver_info;
}
bool is_end_valid() const {
return (!use_virtual_end && end_bone.bone != -1) || (use_virtual_end && !Math::is_zero_approx(end_bone_length));
}
int get_end_bone() const {
return use_virtual_end ? middle_bone.bone : end_bone.bone; // Hack, but useful for external class such as TwoBoneIK3DGizmoPlugin.
}
Vector3 get_pole_direction_vector() const {
Vector3 ret;
switch (pole_direction) {
case SECONDARY_DIRECTION_NONE:
ret = Vector3(0, 0, 0);
break;
case SECONDARY_DIRECTION_PLUS_X:
ret = Vector3(1, 0, 0);
break;
case SECONDARY_DIRECTION_MINUS_X:
ret = Vector3(-1, 0, 0);
break;
case SECONDARY_DIRECTION_PLUS_Y:
ret = Vector3(0, 1, 0);
break;
case SECONDARY_DIRECTION_MINUS_Y:
ret = Vector3(0, -1, 0);
break;
case SECONDARY_DIRECTION_PLUS_Z:
ret = Vector3(0, 0, 1);
break;
case SECONDARY_DIRECTION_MINUS_Z:
ret = Vector3(0, 0, -1);
break;
case SECONDARY_DIRECTION_CUSTOM:
ret = pole_direction_vector;
break;
}
return ret;
}
void cache_current_vectors(Skeleton3D *p_skeleton) {
if (!is_valid()) {
return;
}
root_joint_solver_info->current_vector = (mid_pos - root_pos).normalized();
mid_joint_solver_info->current_vector = (end_pos - mid_pos).normalized();
}
void init_current_joint_rotations(Skeleton3D *p_skeleton) {
if (!is_valid()) {
return;
}
Quaternion parent_gpose;
int parent = p_skeleton->get_bone_parent(root_bone.bone);
if (parent >= 0) {
parent_gpose = p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion();
}
root_joint_solver_info->current_lrest = p_skeleton->get_bone_pose(root_bone.bone).basis.get_rotation_quaternion();
root_joint_solver_info->current_grest = parent_gpose * root_joint_solver_info->current_lrest;
root_joint_solver_info->current_grest.normalize();
root_joint_solver_info->current_lpose = p_skeleton->get_bone_pose(root_bone.bone).basis.get_rotation_quaternion();
root_joint_solver_info->current_gpose = parent_gpose * root_joint_solver_info->current_lpose;
root_joint_solver_info->current_gpose.normalize();
parent_gpose = root_joint_solver_info->current_gpose;
// Mid joint pose is relative to the root joint pose.
mid_joint_solver_info->current_lrest = p_skeleton->get_bone_global_pose(root_bone.bone).basis.get_rotation_quaternion().inverse() * p_skeleton->get_bone_global_pose(middle_bone.bone).basis.get_rotation_quaternion();
mid_joint_solver_info->current_grest = parent_gpose * mid_joint_solver_info->current_lrest;
mid_joint_solver_info->current_grest.normalize();
mid_joint_solver_info->current_lpose = p_skeleton->get_bone_global_pose(root_bone.bone).basis.get_rotation_quaternion().inverse() * p_skeleton->get_bone_global_pose(middle_bone.bone).basis.get_rotation_quaternion();
mid_joint_solver_info->current_gpose = parent_gpose * mid_joint_solver_info->current_lpose;
mid_joint_solver_info->current_gpose.normalize();
cache_current_vectors(p_skeleton);
}
// Make rotation as bone pose from chain coordinates.
void cache_current_joint_rotations(Skeleton3D *p_skeleton, const Vector3 &p_pole_destination) {
if (!is_valid()) {
return;
}
Quaternion parent_gpose;
int parent = p_skeleton->get_bone_parent(root_bone.bone);
if (parent >= 0) {
parent_gpose = p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion();
}
root_joint_solver_info->current_lrest = p_skeleton->get_bone_pose(root_bone.bone).basis.get_rotation_quaternion();
root_joint_solver_info->current_grest = parent_gpose * root_joint_solver_info->current_lrest;
root_joint_solver_info->current_grest.normalize();
Vector3 from = root_joint_solver_info->forward_vector;
Vector3 to = root_joint_solver_info->current_grest.xform_inv(root_joint_solver_info->current_vector).normalized();
root_joint_solver_info->current_lpose = root_joint_solver_info->current_lrest * get_swing(Quaternion(from, to), from);
root_joint_solver_info->current_gpose = parent_gpose * root_joint_solver_info->current_lpose;
root_joint_solver_info->current_gpose.normalize();
Quaternion root_gpose = root_joint_solver_info->current_gpose;
// Mid joint pose is relative to the root joint pose for the case root-mid or mid-end have more than 1 joints.
mid_joint_solver_info->current_lrest = p_skeleton->get_bone_global_pose(root_bone.bone).basis.get_rotation_quaternion().inverse() * p_skeleton->get_bone_global_pose(middle_bone.bone).basis.get_rotation_quaternion();
mid_joint_solver_info->current_grest = root_gpose * mid_joint_solver_info->current_lrest;
mid_joint_solver_info->current_grest.normalize();
from = mid_joint_solver_info->forward_vector;
to = mid_joint_solver_info->current_grest.xform_inv(mid_joint_solver_info->current_vector).normalized();
mid_joint_solver_info->current_lpose = mid_joint_solver_info->current_lrest * get_swing(Quaternion(from, to), from);
mid_joint_solver_info->current_gpose = root_gpose * mid_joint_solver_info->current_lpose;
mid_joint_solver_info->current_gpose.normalize();
bool is_pole_defined = pole_direction != SECONDARY_DIRECTION_NONE && (pole_direction != SECONDARY_DIRECTION_CUSTOM || !pole_direction_vector.is_zero_approx());
// Fix roll to align pole vector to plane.
if (is_pole_defined) {
// Calc roll angles.
Quaternion root_roll_rot = Quaternion();
Quaternion mid_roll_rot = Quaternion();
// Make roll to align pole_vector onto plane with selecting the point nearer pole_destination.
Vector3 pole_dir = get_projected_normal(root_pos, end_pos, p_pole_destination);
if (pole_dir.is_zero_approx()) {
return;
}
Vector3 a = mid_joint_solver_info->current_vector.normalized(); // Global roll axis (mid forward in current pose).
Vector3 k = mid_joint_solver_info->current_gpose.xform(get_pole_direction_vector()).normalized(); // Global pole vector.
Vector3 n = pole_dir.cross((mid_pos - root_pos).normalized()).normalized(); // Global plane normal.
// Guard: degenerate cases (zero or already parallel)
if (a.is_zero_approx() || k.is_zero_approx() || n.is_zero_approx() || Math::is_zero_approx(n.dot(k))) {
return;
}
// c0 cosθ + c1 sinθ + c2 = 0
double c0 = n.dot(k - a * k.dot(a)); // n·(k⊥a)
double c1 = n.dot(a.cross(k)); // n·(a×k)
double c2 = n.dot(a) * k.dot(a); // (n·a)(k·a)
double r = Math::sqrt(c0 * c0 + c1 * c1);
double cos_arg = CLAMP(-c2 / r, -1.0, 1.0);
double phi = Math::atan2(c1, c0);
double acosv = Math::acos(cos_arg);
// Two candidate angles.
double t1 = phi + acosv;
double t2 = phi - acosv;
Quaternion q1(a, t1);
Quaternion q2(a, t2);
// Choose the one whose projected pole points closer to pole side.
Vector3 pole_proj = snap_vector_to_plane(n, pole_dir).normalized();
Vector3 k1p = snap_vector_to_plane(n, q1.xform(k)).normalized();
Vector3 k2p = snap_vector_to_plane(n, q2.xform(k)).normalized();
double s1 = pole_proj.is_zero_approx() ? Math::abs(t1) : k1p.dot(pole_proj);
double s2 = pole_proj.is_zero_approx() ? Math::abs(t2) : k2p.dot(pole_proj);
double t = s1 >= s2 ? t1 : t2;
root_roll_rot = Quaternion(root_joint_solver_info->forward_vector, t);
mid_roll_rot = Quaternion(mid_joint_solver_info->forward_vector, t);
root_joint_solver_info->current_lpose = root_joint_solver_info->current_lpose * root_roll_rot;
root_joint_solver_info->current_gpose = parent_gpose * root_joint_solver_info->current_lpose;
root_joint_solver_info->current_gpose.normalize();
root_gpose = root_joint_solver_info->current_gpose;
mid_joint_solver_info->current_lpose = root_roll_rot.inverse() * mid_joint_solver_info->current_lpose * mid_roll_rot;
mid_joint_solver_info->current_gpose = root_gpose * mid_joint_solver_info->current_lpose;
mid_joint_solver_info->current_gpose.normalize();
}
}
};
protected:
LocalVector<TwoBoneIK3DSetting *> tb_settings;
bool _get(const StringName &p_path, Variant &r_ret) const;
bool _set(const StringName &p_path, const Variant &p_value);
void _get_property_list(List<PropertyInfo> *p_list) const;
void _validate_dynamic_prop(PropertyInfo &p_property) const;
static void _bind_methods();
virtual void _validate_bone_names() override;
void _validate_pole_directions(Skeleton3D *p_skeleton) const;
void _validate_pole_direction(Skeleton3D *p_skeleton, int p_index) const;
virtual void _make_all_joints_dirty() override;
virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override;
virtual void _update_joints(int p_index) override;
virtual void _make_simulation_dirty(int p_index) override;
virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override;
void _process_joints(double p_delta, Skeleton3D *p_skeleton, TwoBoneIK3DSetting *p_setting, const Vector3 &p_destination, const Vector3 &p_pole_destination);
public:
virtual PackedStringArray get_configuration_warnings() const override;
virtual void set_setting_count(int p_count) override {
_set_setting_count<TwoBoneIK3DSetting>(p_count);
tb_settings = _cast_settings<TwoBoneIK3DSetting>();
}
virtual void clear_settings() override {
_set_setting_count<TwoBoneIK3DSetting>(0);
tb_settings.clear();
}
// Setting.
void set_root_bone_name(int p_index, const String &p_bone_name);
String get_root_bone_name(int p_index) const;
void set_root_bone(int p_index, int p_bone);
int get_root_bone(int p_index) const;
void set_middle_bone_name(int p_index, const String &p_bone_name);
String get_middle_bone_name(int p_index) const;
void set_middle_bone(int p_index, int p_bone);
int get_middle_bone(int p_index) const;
void set_end_bone_name(int p_index, const String &p_bone_name);
String get_end_bone_name(int p_index) const;
void set_end_bone(int p_index, int p_bone);
int get_end_bone(int p_index) const;
void set_use_virtual_end(int p_index, bool p_enabled);
bool is_using_virtual_end(int p_index) const;
void set_extend_end_bone(int p_index, bool p_enabled);
bool is_end_bone_extended(int p_index) const;
void set_end_bone_direction(int p_index, BoneDirection p_bone_direction);
BoneDirection get_end_bone_direction(int p_index) const;
void set_end_bone_length(int p_index, float p_length);
float get_end_bone_length(int p_index) const;
void set_target_node(int p_index, const NodePath &p_target_node);
NodePath get_target_node(int p_index) const;
void set_pole_node(int p_index, const NodePath &p_pole_node);
NodePath get_pole_node(int p_index) const;
void set_pole_direction(int p_index, SecondaryDirection p_axis);
SecondaryDirection get_pole_direction(int p_index) const;
void set_pole_direction_vector(int p_index, const Vector3 &p_vector);
Vector3 get_pole_direction_vector(int p_index) const;
bool is_valid(int p_index) const; // Helper for editor and validation.
~TwoBoneIK3D();
};