diff --git a/modules/many_bone_ik/.gitrepo b/modules/many_bone_ik/.gitrepo new file mode 100644 index 000000000000..d83c87441f86 --- /dev/null +++ b/modules/many_bone_ik/.gitrepo @@ -0,0 +1,12 @@ +; DO NOT EDIT (unless you know what you are doing) +; +; This subdirectory is a git "subrepo", and this file is maintained by the +; git-subrepo command. See https://github.com/ingydotnet/git-subrepo#readme +; +[subrepo] + remote = https://github.com/V-Sekai/many_bone_ik + branch = master + commit = 820b56d5dcc1eac585cbf0b04fd4778a6f93bcf7 + parent = f4c7bf6c9c54f1c5370fa4bc331671a7f542ad60 + method = merge + cmdver = 0.4.6 diff --git a/modules/many_bone_ik/AUTHORS.md b/modules/many_bone_ik/AUTHORS.md new file mode 100644 index 000000000000..30baae095ea3 --- /dev/null +++ b/modules/many_bone_ik/AUTHORS.md @@ -0,0 +1,9 @@ +# Godot Engine Many Bone IK authors + +GitHub usernames are indicated in parentheses. + +## Initial Project + +This project is originally a port from a Java project by Eron + + Eron Gjoni (EGjoni) https://github.com/EGjoni/Everything-Will-Be-IK diff --git a/modules/many_bone_ik/README.md b/modules/many_bone_ik/README.md new file mode 100644 index 000000000000..5fdcadbc2927 --- /dev/null +++ b/modules/many_bone_ik/README.md @@ -0,0 +1,13 @@ +# Many Bone IK + +A custom inverse kinematics system solver for multi-chain skeletons and with constraints. + +Lyuma mentioned that there are multiple plausible solutions, especially for dance, where people move internal joints in unusual ways. However, for everything other than dance, there is typically just one right answer - the energy minimizing one. + +Lyuma also agreed that energy minimization is a good concept, as it results in the optimal solution and aligns with what your mind would choose without specific intent. + +Eron explained that with the latest ewb-ik version, energy minimization is handled by defining 0 cost regions in kusudama constraints along with hard boundary (high cost regions). + +Joints always negotiate between moving toward their own 0 cost region and toward the goal, with the former being less important than the latter. The "goal" here refers to whatever orientation allows descendant joints to reach their target. + +Migrated to https://github.com/godotengine/godot/pull/70887. diff --git a/modules/many_bone_ik/SCsub b/modules/many_bone_ik/SCsub new file mode 100644 index 000000000000..300e91bf7079 --- /dev/null +++ b/modules/many_bone_ik/SCsub @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +Import("env") +env_many_bone_ik = env.Clone() +env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik"]) +env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik/src/math"]) +env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik/src"]) +env_many_bone_ik.add_source_files(env.modules_sources, "constraints/*.cpp") +env_many_bone_ik.add_source_files(env.modules_sources, "src/math/*.cpp") +env_many_bone_ik.add_source_files(env.modules_sources, "src/*.cpp") +env_many_bone_ik.add_source_files(env.modules_sources, "*.cpp") + +if env.editor_build: + env_many_bone_ik.add_source_files(env.modules_sources, "editor/*.cpp") diff --git a/modules/many_bone_ik/register_types.h b/modules/many_bone_ik/register_types.h new file mode 100644 index 000000000000..7bb117a1b9a3 --- /dev/null +++ b/modules/many_bone_ik/register_types.h @@ -0,0 +1,39 @@ +/**************************************************************************/ +/* register_types.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. */ +/**************************************************************************/ + +#ifndef MANY_BONE_IK_REGISTER_TYPES_H +#define MANY_BONE_IK_REGISTER_TYPES_H + +#include "modules/register_module_types.h" + +void initialize_many_bone_ik_module(ModuleInitializationLevel p_level); +void uninitialize_many_bone_ik_module(ModuleInitializationLevel p_level); + +#endif // MANY_BONE_IK_REGISTER_TYPES_H diff --git a/modules/many_bone_ik/src/ik_bone_3d.cpp b/modules/many_bone_ik/src/ik_bone_3d.cpp new file mode 100644 index 000000000000..9930b08ef43d --- /dev/null +++ b/modules/many_bone_ik/src/ik_bone_3d.cpp @@ -0,0 +1,325 @@ +/**************************************************************************/ +/* ik_bone_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_bone_3d.h" +#include "ik_kusudama_3d.h" +#include "many_bone_ik_3d.h" +#include "math/ik_node_3d.h" +#include + +void IKBone3D::set_bone_id(BoneId p_bone_id, Skeleton3D *p_skeleton) { + ERR_FAIL_NULL(p_skeleton); + bone_id = p_bone_id; +} + +BoneId IKBone3D::get_bone_id() const { + return bone_id; +} + +void IKBone3D::set_parent(const Ref &p_parent) { + ERR_FAIL_NULL(p_parent); + parent = p_parent; + if (parent.is_valid()) { + parent->children.push_back(this); + godot_skeleton_aligned_transform->set_parent(parent->godot_skeleton_aligned_transform); + constraint_orientation_transform->set_parent(parent->godot_skeleton_aligned_transform); + constraint_twist_transform->set_parent(parent->godot_skeleton_aligned_transform); + } +} + +void IKBone3D::update_default_bone_direction_transform(Skeleton3D *p_skeleton) { + Vector3 child_centroid; + int child_count = 0; + + for (Ref &ik_bone : children) { + child_centroid += ik_bone->get_ik_transform()->get_global_transform().origin; + child_count++; + } + + if (child_count > 0) { + child_centroid /= child_count; + } else { + const PackedInt32Array &bone_children = p_skeleton->get_bone_children(bone_id); + for (BoneId child_bone_idx : bone_children) { + child_centroid += p_skeleton->get_bone_global_pose(child_bone_idx).origin; + } + child_centroid /= bone_children.size(); + } + + const Vector3 &godot_bone_origin = godot_skeleton_aligned_transform->get_global_transform().origin; + child_centroid -= godot_bone_origin; + + if (Math::is_zero_approx(child_centroid.length_squared())) { + if (parent.is_valid()) { + child_centroid = parent->get_bone_direction_transform()->get_global_transform().basis.get_column(Vector3::AXIS_Y); + } else { + child_centroid = get_bone_direction_transform()->get_global_transform().basis.get_column(Vector3::AXIS_Y); + } + } + + if (!Math::is_zero_approx(child_centroid.length_squared()) && (children.size() || p_skeleton->get_bone_children(bone_id).size())) { + child_centroid.normalize(); + Vector3 bone_direction = bone_direction_transform->get_global_transform().basis.get_column(Vector3::AXIS_Y); + bone_direction.normalize(); + bone_direction_transform->rotate_local_with_global(Quaternion(child_centroid, bone_direction)); + } +} + +void IKBone3D::update_default_constraint_transform() { + Ref parent_bone = get_parent(); + if (parent_bone.is_valid()) { + Transform3D parent_bone_aligned_transform = parent_bone->get_ik_transform()->get_global_transform(); + parent_bone_aligned_transform.origin = get_bone_direction_transform()->get_global_transform().origin; + constraint_orientation_transform->set_global_transform(parent_bone_aligned_transform); + } + Transform3D set_constraint_twist_transform = constraint_orientation_transform->get_global_transform(); + constraint_twist_transform->set_global_transform(set_constraint_twist_transform); + + if (constraint.is_null()) { + return; + } + + TypedArray cones = constraint->get_limit_cones(); + Vector3 direction; + + if (cones.size() == 0) { + direction = bone_direction_transform->get_global_transform().basis.get_column(Vector3::AXIS_Y); + } else { + float total_radius_sum = 0.0f; + for (int32_t cone_i = 0; cone_i < cones.size(); cone_i++) { + const Ref &cone = cones[cone_i]; + if (cone.is_null()) { + break; + } + total_radius_sum += cone->get_radius(); + } + + for (int32_t cone_i = 0; cone_i < cones.size(); cone_i++) { + const Ref &cone = cones[cone_i]; + if (cone.is_null()) { + break; + } + float weight = cone->get_radius() / total_radius_sum; + direction += cone->get_control_point() * weight; + } + direction.normalize(); + direction = constraint_orientation_transform->get_global_transform().xform(direction); + direction -= constraint_orientation_transform->get_global_transform().origin; + } + + Vector3 twist_axis = constraint_twist_transform->get_global_transform().basis.get_column(Vector3::AXIS_Y); + Quaternion align_dir = Quaternion(twist_axis, direction); + constraint_twist_transform->rotate_local_with_global(align_dir); +} + +Ref IKBone3D::get_parent() const { + return parent; +} + +void IKBone3D::set_pin(const Ref &p_pin) { + ERR_FAIL_NULL(p_pin); + pin = p_pin; +} + +Ref IKBone3D::get_pin() const { + return pin; +} + +void IKBone3D::set_pose(const Transform3D &p_transform) { + godot_skeleton_aligned_transform->set_transform(p_transform); +} + +Transform3D IKBone3D::get_pose() const { + return godot_skeleton_aligned_transform->get_transform(); +} + +void IKBone3D::set_global_pose(const Transform3D &p_transform) { + godot_skeleton_aligned_transform->set_global_transform(p_transform); + Transform3D transform = constraint_orientation_transform->get_transform(); + transform.origin = godot_skeleton_aligned_transform->get_transform().origin; + constraint_orientation_transform->set_transform(transform); + constraint_orientation_transform->_propagate_transform_changed(); +} + +Transform3D IKBone3D::get_global_pose() const { + return godot_skeleton_aligned_transform->get_global_transform(); +} + +Transform3D IKBone3D::get_bone_direction_global_pose() const { + return bone_direction_transform->get_global_transform(); +} + +void IKBone3D::set_initial_pose(Skeleton3D *p_skeleton) { + ERR_FAIL_NULL(p_skeleton); + if (bone_id == -1) { + return; + } + Transform3D bone_origin_to_parent_origin = p_skeleton->get_bone_pose(bone_id); + set_pose(bone_origin_to_parent_origin); +} + +void IKBone3D::set_skeleton_bone_pose(Skeleton3D *p_skeleton) { + ERR_FAIL_NULL(p_skeleton); + Transform3D bone_to_parent = get_pose(); + p_skeleton->set_bone_pose_position(bone_id, bone_to_parent.origin); + if (!bone_to_parent.basis.is_finite()) { + bone_to_parent.basis = Basis(); + } + p_skeleton->set_bone_pose_rotation(bone_id, bone_to_parent.basis.get_rotation_quaternion()); + // The ik solver doesn't modify scale, and if we do it'll be to orthonormalize which will break the underlying skeleton. + // p_skeleton->set_bone_pose_scale(bone_id, bone_to_parent.basis.get_scale()); +} + +void IKBone3D::create_pin() { + pin = Ref(memnew(IKEffector3D(this))); +} + +bool IKBone3D::is_pinned() const { + return pin.is_valid(); +} + +void IKBone3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_pin"), &IKBone3D::get_pin); + ClassDB::bind_method(D_METHOD("set_pin", "pin"), &IKBone3D::set_pin); + ClassDB::bind_method(D_METHOD("is_pinned"), &IKBone3D::is_pinned); + ClassDB::bind_method(D_METHOD("get_constraint"), &IKBone3D::get_constraint); + ClassDB::bind_method(D_METHOD("get_constraint_orientation_transform"), &IKBone3D::get_constraint_orientation_transform); + ClassDB::bind_method(D_METHOD("get_constraint_twist_transform"), &IKBone3D::get_constraint_twist_transform); +} + +IKBone3D::IKBone3D(StringName p_bone, Skeleton3D *p_skeleton, const Ref &p_parent, Vector> &p_pins, float p_default_dampening, + ManyBoneIK3D *p_many_bone_ik) { + ERR_FAIL_NULL(p_skeleton); + + default_dampening = p_default_dampening; + cos_half_dampen = cos(default_dampening / real_t(2.0)); + set_name(p_bone); + bone_id = p_skeleton->find_bone(p_bone); + if (p_parent.is_valid()) { + set_parent(p_parent); + } + for (Ref elem : p_pins) { + if (elem.is_null()) { + continue; + } + if (elem->get_name() == p_bone && !elem->get_target_node().is_empty() && p_many_bone_ik && p_many_bone_ik->get_node_or_null(elem->get_target_node())) { + create_pin(); + Ref effector = get_pin(); + effector->set_target_node(p_skeleton, elem->get_target_node()); + effector->set_passthrough_factor(elem->get_passthrough_factor()); + effector->set_weight(elem->get_weight()); + effector->set_direction_priorities(elem->get_direction_priorities()); + break; + } + } + bone_direction_transform->set_parent(godot_skeleton_aligned_transform); + + float predamp = 1.0 - get_stiffness(); + dampening = get_parent().is_null() ? Math_PI : predamp * p_default_dampening; + float iterations = p_many_bone_ik->get_iterations_per_frame(); + if (get_constraint().is_null()) { + Ref new_constraint; + new_constraint.instantiate(); + add_constraint(new_constraint); + } + float returnfulness = get_constraint()->get_resistance(); + float falloff = 0.2f; + half_returnfulness_dampened.resize(iterations); + cos_half_returnfulness_dampened.resize(iterations); + float iterations_pow = Math::pow(iterations, falloff * iterations * returnfulness); + for (float i = 0; i < iterations; i++) { + float iteration_scalar = ((iterations_pow)-Math::pow(i, falloff * iterations * returnfulness)) / (iterations_pow); + float iteration_return_clamp = iteration_scalar * returnfulness * dampening; + float cos_iteration_return_clamp = Math::cos(iteration_return_clamp / 2.0); + half_returnfulness_dampened.write[i] = iteration_return_clamp; + cos_half_returnfulness_dampened.write[i] = cos_iteration_return_clamp; + } +} + +float IKBone3D::get_cos_half_dampen() const { + return cos_half_dampen; +} + +void IKBone3D::set_cos_half_dampen(float p_cos_half_dampen) { + cos_half_dampen = p_cos_half_dampen; +} + +Ref IKBone3D::get_constraint() const { + return constraint; +} + +void IKBone3D::add_constraint(Ref p_constraint) { + constraint = p_constraint; +} + +Ref IKBone3D::get_ik_transform() { + return godot_skeleton_aligned_transform; +} + +Ref IKBone3D::get_constraint_orientation_transform() { + return constraint_orientation_transform; +} + +Ref IKBone3D::get_constraint_twist_transform() { + return constraint_twist_transform; +} + +void IKBone3D::set_constraint_orientation_transform(Ref p_transform) { + constraint_orientation_transform = p_transform; +} + +void IKBone3D::set_bone_direction_transform(Ref p_bone_direction) { + bone_direction_transform = p_bone_direction; +} + +Ref IKBone3D::get_bone_direction_transform() { + return bone_direction_transform; +} + +bool IKBone3D::is_orientationally_constrained() { + if (get_constraint().is_null()) { + return false; + } + return get_constraint()->is_orientationally_constrained(); +} + +bool IKBone3D::is_axially_constrained() { + if (get_constraint().is_null()) { + return false; + } + return get_constraint()->is_axially_constrained(); +} + +void IKBone3D::pull_back_toward_allowable_region() { + Ref current_constraint = get_constraint(); + if (current_constraint.is_valid()) { + current_constraint->set_axes_to_returnfulled(get_bone_direction_transform(), get_ik_transform(), get_constraint_orientation_transform(), cos_half_return_damp, return_damp); + } +} diff --git a/modules/many_bone_ik/src/ik_bone_3d.h b/modules/many_bone_ik/src/ik_bone_3d.h new file mode 100644 index 000000000000..be799e4c3c55 --- /dev/null +++ b/modules/many_bone_ik/src/ik_bone_3d.h @@ -0,0 +1,135 @@ +/**************************************************************************/ +/* ik_bone_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. */ +/**************************************************************************/ + +#ifndef IK_BONE_3D_H +#define IK_BONE_3D_H + +#include "ik_effector_template_3d.h" +#include "ik_kusudama_3d.h" +#include "ik_limit_cone_3d.h" +#include "math/ik_node_3d.h" + +#include "core/io/resource.h" +#include "core/object/ref_counted.h" +#include "scene/3d/skeleton_3d.h" + +class IKEffector3D; +class ManyBoneIK3D; +class IKBone3D; + +class IKBone3D : public Resource { + GDCLASS(IKBone3D, Resource); + + BoneId bone_id = -1; + Ref parent; + Vector> children; + Ref pin; + + float default_dampening = Math_PI; + float dampening = get_parent().is_null() ? Math_PI : default_dampening; + float cos_half_dampen = Math::cos(dampening / 2.0f); + double cos_half_return_damp = 0.0f; + double return_damp = 0.0f; + Vector cos_half_returnfulness_dampened; + Vector half_returnfulness_dampened; + double stiffness = 0.0; + Ref constraint; + // In the space of the local parent bone transform. + // The origin is the origin of the bone direction transform + // Can be independent and should be calculated + // to keep -y to be the opposite of its bone forward orientation + // To avoid singularity that is ambiguous. + Ref constraint_orientation_transform = Ref(memnew(IKNode3D())); + Ref constraint_twist_transform = Ref(memnew(IKNode3D())); + Ref godot_skeleton_aligned_transform = Ref(memnew(IKNode3D())); // The bone's actual transform. + Ref bone_direction_transform = Ref(memnew(IKNode3D())); // Physical direction of the bone. Calculate Y is the bone up. + +protected: + static void _bind_methods(); + +public: + Vector &getCosHalfReturnfullnessDampened() { + return cos_half_returnfulness_dampened; + } + + void setCosHalfReturnfullnessDampened(const Vector &value) { + cos_half_returnfulness_dampened = value; + } + + Vector &getHalfReturnfullnessDampened() { + return half_returnfulness_dampened; + } + + void setHalfReturnfullnessDampened(const Vector &value) { + half_returnfulness_dampened = value; + } + void set_stiffness(double p_stiffness) { + stiffness = p_stiffness; + } + + double get_stiffness() const { + return stiffness; + } + void pull_back_toward_allowable_region(); + bool is_axially_constrained(); + bool is_orientationally_constrained(); + Transform3D get_bone_direction_global_pose() const; + Ref get_bone_direction_transform(); + void set_bone_direction_transform(Ref p_bone_direction); + void update_default_bone_direction_transform(Skeleton3D *p_skeleton); + void set_constraint_orientation_transform(Ref p_transform); + Ref get_constraint_orientation_transform(); + Ref get_constraint_twist_transform(); + void update_default_constraint_transform(); + void add_constraint(Ref p_constraint); + Ref get_constraint() const; + void set_bone_id(BoneId p_bone_id, Skeleton3D *p_skeleton = nullptr); + BoneId get_bone_id() const; + void set_parent(const Ref &p_parent); + Ref get_parent() const; + void set_pin(const Ref &p_pin); + Ref get_pin() const; + void set_global_pose(const Transform3D &p_transform); + Transform3D get_global_pose() const; + void set_pose(const Transform3D &p_transform); + Transform3D get_pose() const; + void set_initial_pose(Skeleton3D *p_skeleton); + void set_skeleton_bone_pose(Skeleton3D *p_skeleton); + void create_pin(); + bool is_pinned() const; + Ref get_ik_transform(); + IKBone3D() {} + IKBone3D(StringName p_bone, Skeleton3D *p_skeleton, const Ref &p_parent, Vector> &p_pins, float p_default_dampening = Math_PI, ManyBoneIK3D *p_many_bone_ik = nullptr); + ~IKBone3D() {} + float get_cos_half_dampen() const; + void set_cos_half_dampen(float p_cos_half_dampen); +}; + +#endif // IK_BONE_3D_H diff --git a/modules/many_bone_ik/src/ik_bone_segment_3d.cpp b/modules/many_bone_ik/src/ik_bone_segment_3d.cpp new file mode 100644 index 000000000000..6d42e4ea55e5 --- /dev/null +++ b/modules/many_bone_ik/src/ik_bone_segment_3d.cpp @@ -0,0 +1,488 @@ +/**************************************************************************/ +/* ik_bone_segment_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_bone_segment_3d.h" + +#include "ik_effector_3d.h" +#include "ik_kusudama_3d.h" +#include "ik_limit_cone_3d.h" +#include "many_bone_ik_3d.h" +#include "math/ik_node_3d.h" +#include "scene/3d/skeleton_3d.h" + +Ref IKBoneSegment3D::get_root() const { + return root; +} +Ref IKBoneSegment3D::get_tip() const { + return tip; +} + +bool IKBoneSegment3D::is_pinned() const { + ERR_FAIL_NULL_V(tip, false); + return tip->is_pinned(); +} + +Vector> IKBoneSegment3D::get_child_segments() const { + return child_segments; +} + +void IKBoneSegment3D::create_bone_list(Vector> &p_list, bool p_recursive, bool p_debug_skeleton) const { + if (p_recursive) { + for (int32_t child_i = 0; child_i < child_segments.size(); child_i++) { + child_segments[child_i]->create_bone_list(p_list, p_recursive, p_debug_skeleton); + } + } + Ref current_bone = tip; + Vector> list; + while (current_bone.is_valid()) { + list.push_back(current_bone); + if (current_bone == root) { + break; + } + current_bone = current_bone->get_parent(); + } + if (p_debug_skeleton) { + for (int32_t name_i = 0; name_i < list.size(); name_i++) { + BoneId bone = list[name_i]->get_bone_id(); + + String bone_name = skeleton->get_bone_name(bone); + String effector; + if (list[name_i]->is_pinned()) { + effector += "Effector "; + } + String prefix; + if (list[name_i] == root) { + prefix += "(" + effector + "Root) "; + } + if (list[name_i] == tip) { + prefix += "(" + effector + "Tip) "; + } + print_line(vformat("%s%s (%s)", prefix, bone_name, itos(bone))); + } + } + p_list.append_array(list); +} + +void IKBoneSegment3D::update_pinned_list(Vector> &r_weights) { + for (int32_t chain_i = 0; chain_i < child_segments.size(); chain_i++) { + Ref chain = child_segments[chain_i]; + chain->update_pinned_list(r_weights); + } + if (is_pinned()) { + effector_list.push_back(tip->get_pin()); + } + double passthrough_factor = is_pinned() ? tip->get_pin()->passthrough_factor : 1.0; + if (passthrough_factor > 0.0) { + for (Ref child : child_segments) { + effector_list.append_array(child->effector_list); + } + } +} + +void IKBoneSegment3D::update_optimal_rotation(Ref p_for_bone, double p_damp, bool p_translate, bool p_constraint_mode, int32_t current_iteration, int32_t total_iterations) { + ERR_FAIL_NULL(p_for_bone); + update_target_headings(p_for_bone, &heading_weights, &target_headings); + update_tip_headings(p_for_bone, &tip_headings); + set_optimal_rotation(p_for_bone, &tip_headings, &target_headings, &heading_weights, p_damp, p_translate, p_constraint_mode); +} + +Quaternion IKBoneSegment3D::clamp_to_quadrance_angle(Quaternion p_quat, double p_cos_half_angle) { + double newCoeff = double(1.0) - (p_cos_half_angle * Math::abs(p_cos_half_angle)); + Quaternion rot = p_quat; + double currentCoeff = rot.x * rot.x + rot.y * rot.y + rot.z * rot.z; + + if (newCoeff >= currentCoeff) { + return rot; + } else { + // Calculate how much over the limit the rotation is, between 0 and 1 + double over_limit = (currentCoeff - newCoeff) / (1.0 - newCoeff); + Quaternion clamped_rotation = rot; + clamped_rotation.w = rot.w < double(0.0) ? -p_cos_half_angle : p_cos_half_angle; + double compositeCoeff = Math::sqrt(newCoeff / currentCoeff); + clamped_rotation.x *= compositeCoeff; + clamped_rotation.y *= compositeCoeff; + clamped_rotation.z *= compositeCoeff; + return rot.slerp(clamped_rotation, over_limit); + } +} + +float IKBoneSegment3D::get_manual_msd(const PackedVector3Array &r_htip, const PackedVector3Array &r_htarget, const Vector &p_weights) { + float manual_RMSD = 0.0f; + float w_sum = 0.0f; + for (int i = 0; i < r_htarget.size(); i++) { + float x_d = r_htarget[i].x - r_htip[i].x; + float y_d = r_htarget[i].y - r_htip[i].y; + float z_d = r_htarget[i].z - r_htip[i].z; + float mag_sq = p_weights[i] * (x_d * x_d + y_d * y_d + z_d * z_d); + manual_RMSD += mag_sq; + w_sum += p_weights[i]; + } + manual_RMSD /= w_sum * w_sum; + return manual_RMSD; +} + +void IKBoneSegment3D::set_optimal_rotation(Ref p_for_bone, PackedVector3Array *r_htip, PackedVector3Array *r_htarget, Vector *r_weights, float p_dampening, bool p_translate, bool p_constraint_mode, int32_t current_iteration, int32_t total_iterations) { + ERR_FAIL_NULL(p_for_bone); + ERR_FAIL_NULL(r_htip); + ERR_FAIL_NULL(r_htarget); + ERR_FAIL_NULL(r_weights); + + update_target_headings(p_for_bone, &heading_weights, &target_headings); + Transform3D prev_transform = p_for_bone->get_pose(); + bool got_closer = true; + double bone_damp = p_for_bone->get_cos_half_dampen(); + + int i = 0; + do { + update_tip_headings(p_for_bone, &tip_headings); + if (!p_constraint_mode) { + // Solved the ik transform and apply it. + QCP qcp = QCP(evec_prec, eval_prec); + Quaternion rot = qcp.weighted_superpose(*r_htip, *r_htarget, *r_weights, p_translate); + Vector3 translation = qcp.get_translation(); + double dampening = (p_dampening != -1.0) ? p_dampening : bone_damp; + rot = clamp_to_quadrance_angle(rot, cos(dampening / 2.0)).normalized(); + p_for_bone->get_ik_transform()->rotate_local_with_global(rot); + Transform3D result = Transform3D(p_for_bone->get_global_pose().basis, p_for_bone->get_global_pose().origin + translation); + result.orthonormalize(); + p_for_bone->set_global_pose(result); + } + // Calculate orientation before twist to avoid exceeding the twist bound when updating the rotation. + if (p_for_bone->is_orientationally_constrained() && p_for_bone->get_parent().is_valid()) { + p_for_bone->get_constraint()->set_axes_to_orientation_snap(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_orientation_transform(), bone_damp, p_for_bone->get_cos_half_dampen()); + } + if (p_for_bone->is_axially_constrained() && p_for_bone->get_parent().is_valid()) { + p_for_bone->get_constraint()->set_snap_to_twist_limit(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_twist_transform(), bone_damp, p_for_bone->get_cos_half_dampen()); + } + + if (default_stabilizing_pass_count > 0) { + update_tip_headings(p_for_bone, &tip_headings_uniform); + double current_msd = get_manual_msd(tip_headings_uniform, target_headings, heading_weights); + if (current_msd <= previous_deviation * 1.0001) { + if (p_for_bone->get_constraint().is_valid() && !Math::is_zero_approx(p_for_bone->get_constraint()->get_resistance())) { + if (bone_damp != -1) { + float returnfulness = p_for_bone->get_constraint()->get_resistance(); + float dampened_angle = p_for_bone->get_stiffness() * bone_damp * returnfulness; + float total_iterations_square = total_iterations * total_iterations; + float scaled_dampened_angle = dampened_angle * ((total_iterations_square - (current_iteration * current_iteration)) / total_iterations_square); + float cos_half_angle = Math::cos(0.5f * scaled_dampened_angle); + p_for_bone->get_constraint()->set_axes_to_returnfulled(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_orientation_transform(), cos_half_angle, scaled_dampened_angle); + } else { + p_for_bone->get_constraint()->set_axes_to_returnfulled(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_orientation_transform(), p_for_bone->getCosHalfReturnfullnessDampened()[current_iteration], p_for_bone->getCosHalfReturnfullnessDampened()[current_iteration]); + } + update_tip_headings(p_for_bone, &tip_headings); + current_msd = get_manual_msd(tip_headings, target_headings, heading_weights); + } + previous_deviation = current_msd; + got_closer = true; + break; + } else { + got_closer = false; + } + } + i++; + } while (i < default_stabilizing_pass_count && !got_closer); + + if (!got_closer) { + p_for_bone->set_pose(prev_transform); + } + + if (root == p_for_bone) { + previous_deviation = INFINITY; + } +} + +void IKBoneSegment3D::update_target_headings(Ref p_for_bone, Vector *r_weights, PackedVector3Array *r_target_headings) { + ERR_FAIL_NULL(p_for_bone); + ERR_FAIL_NULL(r_weights); + ERR_FAIL_NULL(r_target_headings); + int32_t last_index = 0; + for (int32_t effector_i = 0; effector_i < effector_list.size(); effector_i++) { + Ref effector = effector_list[effector_i]; + if (effector.is_null()) { + continue; + } + last_index = effector->update_effector_target_headings(r_target_headings, last_index, p_for_bone, &heading_weights); + } +} + +void IKBoneSegment3D::update_tip_headings(Ref p_for_bone, PackedVector3Array *r_heading_tip) { + ERR_FAIL_NULL(r_heading_tip); + ERR_FAIL_NULL(p_for_bone); + int32_t last_index = 0; + for (int32_t effector_i = 0; effector_i < effector_list.size(); effector_i++) { + Ref effector = effector_list[effector_i]; + if (effector.is_null()) { + continue; + } + last_index = effector->update_effector_tip_headings(r_heading_tip, last_index, p_for_bone); + } +} + +void IKBoneSegment3D::segment_solver(const Vector &p_damp, float p_default_damp, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iteration) { + for (Ref child : child_segments) { + if (child.is_null()) { + continue; + } + child->segment_solver(p_damp, p_default_damp, p_constraint_mode, p_current_iteration, p_total_iteration); + } + bool is_translate = parent_segment.is_null(); + if (is_translate) { + Vector damp = p_damp; + damp.fill(Math_PI); + qcp_solver(damp, Math_PI, is_translate, p_constraint_mode, p_current_iteration, p_total_iteration); + return; + } + qcp_solver(p_damp, p_default_damp, is_translate, p_constraint_mode, p_current_iteration, p_total_iteration); +} + +void IKBoneSegment3D::qcp_solver(const Vector &p_damp, float p_default_damp, bool p_translate, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iterations) { + for (Ref current_bone : bones) { + float damp = p_default_damp; + bool is_valid_access = !(unlikely((p_damp.size()) < 0 || (current_bone->get_bone_id()) >= (p_damp.size()))); + if (is_valid_access) { + damp = p_damp[current_bone->get_bone_id()]; + } + bool is_non_default_damp = p_default_damp < damp; + if (is_non_default_damp) { + damp = p_default_damp; + } + update_optimal_rotation(current_bone, damp, p_translate && current_bone == root, p_constraint_mode, p_current_iteration, p_total_iterations); + } +} + +void IKBoneSegment3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("is_pinned"), &IKBoneSegment3D::is_pinned); + ClassDB::bind_method(D_METHOD("get_ik_bone", "bone"), &IKBoneSegment3D::get_ik_bone); +} + +Ref IKBoneSegment3D::get_parent_segment() { + return parent_segment; +} + +IKBoneSegment3D::IKBoneSegment3D(Skeleton3D *p_skeleton, StringName p_root_bone_name, Vector> &p_pins, ManyBoneIK3D *p_many_bone_ik, const Ref &p_parent, + BoneId p_root, BoneId p_tip, int32_t p_stabilizing_pass_count) { + root = p_root; + tip = p_tip; + skeleton = p_skeleton; + root = Ref(memnew(IKBone3D(p_root_bone_name, p_skeleton, p_parent, p_pins, Math_PI, p_many_bone_ik))); + if (p_parent.is_valid()) { + root_segment = p_parent->root_segment; + } else { + root_segment = Ref(this); + } + root_segment->bone_map[root->get_bone_id()] = root; + if (p_parent.is_valid()) { + parent_segment = p_parent; + root->set_parent(p_parent->get_tip()); + } + default_stabilizing_pass_count = p_stabilizing_pass_count; +} + +void IKBoneSegment3D::enable_pinned_descendants() { + pinned_descendants = true; +} + +bool IKBoneSegment3D::has_pinned_descendants() { + return pinned_descendants; +} + +Vector> IKBoneSegment3D::get_bone_list() const { + return bones; +} + +Ref IKBoneSegment3D::get_ik_bone(BoneId p_bone) const { + if (!bone_map.has(p_bone)) { + return Ref(); + } + return bone_map[p_bone]; +} + +void IKBoneSegment3D::create_headings_arrays() { + Vector> penalty_array; + Vector> new_pinned_bones; + recursive_create_penalty_array(this, penalty_array, new_pinned_bones, 1.0); + pinned_bones.resize(new_pinned_bones.size()); + int32_t total_headings = 0; + for (const Vector ¤t_penalty_array : penalty_array) { + total_headings += current_penalty_array.size(); + } + for (int32_t bone_i = 0; bone_i < new_pinned_bones.size(); bone_i++) { + pinned_bones.write[bone_i] = new_pinned_bones[bone_i]; + } + target_headings.resize(total_headings); + tip_headings.resize(total_headings); + tip_headings_uniform.resize(total_headings); + heading_weights.resize(total_headings); + int currentHeading = 0; + for (const Vector ¤t_penalty_array : penalty_array) { + for (double ad : current_penalty_array) { + heading_weights.write[currentHeading] = ad; + target_headings.write[currentHeading] = Vector3(); + tip_headings.write[currentHeading] = Vector3(); + tip_headings_uniform.write[currentHeading] = Vector3(); + currentHeading++; + } + } +} + +void IKBoneSegment3D::recursive_create_penalty_array(Ref p_bone_segment, Vector> &r_penalty_array, Vector> &r_pinned_bones, double p_falloff) { + if (p_falloff <= 0.0) { + return; + } + + double current_falloff = 1.0; + + if (p_bone_segment->is_pinned()) { + Ref current_tip = p_bone_segment->get_tip(); + Ref pin = current_tip->get_pin(); + double weight = pin->get_weight(); + Vector inner_weight_array; + inner_weight_array.push_back(weight * p_falloff); + + double max_pin_weight = MAX(MAX(pin->get_direction_priorities().x, pin->get_direction_priorities().y), pin->get_direction_priorities().z); + max_pin_weight = max_pin_weight == 0.0 ? 1.0 : max_pin_weight; + + for (int i = 0; i < 3; ++i) { + double priority = pin->get_direction_priorities()[i]; + if (priority > 0.0) { + double sub_target_weight = weight * (priority / max_pin_weight) * p_falloff; + inner_weight_array.push_back(sub_target_weight); + inner_weight_array.push_back(sub_target_weight); + } + } + + r_penalty_array.push_back(inner_weight_array); + r_pinned_bones.push_back(current_tip); + current_falloff = pin->get_passthrough_factor(); + } + + for (Ref s : p_bone_segment->get_child_segments()) { + recursive_create_penalty_array(s, r_penalty_array, r_pinned_bones, p_falloff * current_falloff); + } +} + +void IKBoneSegment3D::recursive_create_headings_arrays_for(Ref p_bone_segment) { + p_bone_segment->create_headings_arrays(); + for (Ref segments : p_bone_segment->get_child_segments()) { + recursive_create_headings_arrays_for(segments); + } +} + +void IKBoneSegment3D::generate_default_segments(Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik) { + Ref current_tip = root; + + while (true) { + if (_is_parent_of_tip(current_tip, p_tip_bone)) { + break; + } + + Vector children = skeleton->get_bone_children(current_tip->get_bone_id()); + + if (_has_multiple_children_or_pinned(children, current_tip)) { + _process_children(children, current_tip, p_pins, p_root_bone, p_tip_bone, p_many_bone_ik); + break; + } else if (children.size() == 1) { + current_tip = _create_next_bone(children[0], current_tip, p_pins, p_many_bone_ik); + } else { + break; + } + } + + _finalize_segment(current_tip); +} + +bool IKBoneSegment3D::_is_parent_of_tip(Ref p_current_tip, BoneId p_tip_bone) { + return skeleton->get_bone_parent(p_current_tip->get_bone_id()) >= p_tip_bone && p_tip_bone != -1; +} + +bool IKBoneSegment3D::_has_multiple_children_or_pinned(Vector &r_children, Ref p_current_tip) { + return r_children.size() > 1 || p_current_tip->is_pinned(); +} + +void IKBoneSegment3D::_process_children(Vector &r_children, Ref p_current_tip, Vector> &r_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik) { + tip = p_current_tip; + Ref parent(this); + + for (int32_t child_i = 0; child_i < r_children.size(); child_i++) { + BoneId child_bone = r_children[child_i]; + String child_name = skeleton->get_bone_name(child_bone); + Ref child_segment = _create_child_segment(child_name, r_pins, p_root_bone, p_tip_bone, p_many_bone_ik, parent); + + child_segment->generate_default_segments(r_pins, p_root_bone, p_tip_bone, p_many_bone_ik); + + if (child_segment->has_pinned_descendants()) { + enable_pinned_descendants(); + child_segments.push_back(child_segment); + } + } +} + +Ref IKBoneSegment3D::_create_child_segment(String &p_child_name, Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik, Ref &p_parent) { + return Ref(memnew(IKBoneSegment3D(skeleton, p_child_name, p_pins, p_many_bone_ik, p_parent, p_root_bone, p_tip_bone))); +} + +Ref IKBoneSegment3D::_create_next_bone(BoneId p_bone_id, Ref p_current_tip, Vector> &p_pins, ManyBoneIK3D *p_many_bone_ik) { + String bone_name = skeleton->get_bone_name(p_bone_id); + Ref next_bone = Ref(memnew(IKBone3D(bone_name, skeleton, p_current_tip, p_pins, p_many_bone_ik->get_default_damp(), p_many_bone_ik))); + root_segment->bone_map[p_bone_id] = next_bone; + + return next_bone; +} + +void IKBoneSegment3D::_finalize_segment(Ref p_current_tip) { + tip = p_current_tip; + + if (tip->is_pinned()) { + enable_pinned_descendants(); + } + + set_name(vformat("IKBoneSegment%sRoot%sTip", root->get_name(), tip->get_name())); + bones.clear(); + create_bone_list(bones, false); +} + +int32_t IKBoneSegment3D::get_stabilization_passes() const { + return default_stabilizing_pass_count; +} + +void IKBoneSegment3D::set_stabilization_passes(int32_t p_passes) { + default_stabilizing_pass_count = p_passes; +} + +void IKBoneSegment3D::update_returnfulness_damp(int32_t p_iterations) { + for (Ref bone : bones) { + if (bone.is_null()) { + continue; + } + bone->pull_back_toward_allowable_region(); + previous_deviation = INFINITY; + } +} diff --git a/modules/many_bone_ik/src/ik_bone_segment_3d.h b/modules/many_bone_ik/src/ik_bone_segment_3d.h new file mode 100644 index 000000000000..dd6a43ff620b --- /dev/null +++ b/modules/many_bone_ik/src/ik_bone_segment_3d.h @@ -0,0 +1,113 @@ +/**************************************************************************/ +/* ik_bone_segment_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. */ +/**************************************************************************/ + +#ifndef IK_BONE_SEGMENT_3D_H +#define IK_BONE_SEGMENT_3D_H + +#include "ik_bone_3d.h" +#include "ik_effector_3d.h" +#include "ik_effector_template_3d.h" +#include "math/qcp.h" +#include "scene/3d/skeleton_3d.h" + +#include "core/io/resource.h" +#include "core/object/ref_counted.h" + +class IKEffector3D; +class IKBone3D; +class IKLimitCone3D; + +class IKBoneSegment3D : public Resource { + GDCLASS(IKBoneSegment3D, Resource); + Ref root; + Ref tip; + Vector> bones; + Vector> pinned_bones; + Vector> child_segments; // Contains only direct child chains that end with effectors or have child that end with effectors + Ref parent_segment; + Ref root_segment; + Vector> effector_list; + PackedVector3Array target_headings; + PackedVector3Array tip_headings; + PackedVector3Array tip_headings_uniform; + Vector heading_weights; + BoneId many_bone_ik_tip_bone = -1; + int32_t idx_eff_i = -1, idx_eff_f = -1; + Skeleton3D *skeleton = nullptr; + bool pinned_descendants = false; + double previous_deviation = INFINITY; + int32_t default_stabilizing_pass_count = 0; // Move to the stabilizing pass to the ik solver. Set it free. + bool has_pinned_descendants(); + void enable_pinned_descendants(); + void update_target_headings(Ref p_for_bone, Vector *r_weights, PackedVector3Array *r_htarget); + void update_tip_headings(Ref p_for_bone, PackedVector3Array *r_heading_tip); + void set_optimal_rotation(Ref p_for_bone, PackedVector3Array *r_htip, PackedVector3Array *r_heading_tip, Vector *r_weights, float p_dampening = -1, bool p_translate = false, bool p_constraint_mode = false, int32_t current_iteration = 0, int32_t total_iterations = 0); + void qcp_solver(const Vector &p_damp, float p_default_damp, bool p_translate, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iterations); + void update_optimal_rotation(Ref p_for_bone, double p_damp, bool p_translate, bool p_constraint_mode, int32_t current_iteration, int32_t total_iterations); + float get_manual_msd(const PackedVector3Array &r_htip, const PackedVector3Array &r_htarget, const Vector &p_weights); + HashMap> bone_map; + bool _is_parent_of_tip(Ref p_current_tip, BoneId p_tip_bone); + bool _has_multiple_children_or_pinned(Vector &r_children, Ref p_current_tip); + void _process_children(Vector &r_children, Ref p_current_tip, Vector> &r_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik); + Ref _create_child_segment(String &p_child_name, Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik, Ref &p_parent); + Ref _create_next_bone(BoneId p_bone_id, Ref p_current_tip, Vector> &p_pins, ManyBoneIK3D *p_many_bone_ik); + void _finalize_segment(Ref p_current_tip); + +protected: + static void _bind_methods(); + +public: + const double evec_prec = static_cast(1E-6); + const double eval_prec = static_cast(1E-11); + void update_pinned_list(Vector> &r_weights); + void set_stabilization_passes(int32_t p_passes); + int32_t get_stabilization_passes() const; + static Quaternion clamp_to_quadrance_angle(Quaternion p_quat, double p_cos_half_angle); + static void recursive_create_headings_arrays_for(Ref p_bone_segment); + void create_headings_arrays(); + void recursive_create_penalty_array(Ref p_bone_segment, Vector> &r_penalty_array, Vector> &r_pinned_bones, double p_falloff); + Ref get_parent_segment(); + void segment_solver(const Vector &p_damp, float p_default_damp, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iteration); + Ref get_root() const; + Ref get_tip() const; + bool is_pinned() const; + Vector> get_child_segments() const; + void create_bone_list(Vector> &p_list, bool p_recursive = false, bool p_debug_skeleton = false) const; + Vector> get_bone_list() const; + Ref get_ik_bone(BoneId p_bone) const; + void update_returnfulness_damp(int32_t p_iterations); + void generate_default_segments(Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik); + IKBoneSegment3D() {} + IKBoneSegment3D(Skeleton3D *p_skeleton, StringName p_root_bone_name, Vector> &p_pins, ManyBoneIK3D *p_many_bone_ik, const Ref &p_parent = nullptr, + BoneId root = -1, BoneId tip = -1, int32_t p_stabilizing_pass_count = 0); + ~IKBoneSegment3D() {} +}; + +#endif // IK_BONE_SEGMENT_3D_H diff --git a/modules/many_bone_ik/src/ik_effector_3d.cpp b/modules/many_bone_ik/src/ik_effector_3d.cpp new file mode 100644 index 000000000000..9732bfca0997 --- /dev/null +++ b/modules/many_bone_ik/src/ik_effector_3d.cpp @@ -0,0 +1,186 @@ +/**************************************************************************/ +/* ik_effector_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_effector_3d.h" + +#include "core/typedefs.h" +#include "ik_bone_3d.h" +#include "many_bone_ik_3d.h" +#include "math/ik_node_3d.h" +#include "scene/3d/node_3d.h" + +#ifdef TOOLS_ENABLED +#include "editor/editor_data.h" +#include "editor/editor_node.h" +#endif + +void IKEffector3D::set_target_node(Skeleton3D *p_skeleton, const NodePath &p_target_node_path) { + ERR_FAIL_NULL(p_skeleton); + target_node_path = p_target_node_path; +} + +NodePath IKEffector3D::get_target_node() const { + return target_node_path; +} + +void IKEffector3D::set_target_node_rotation(bool p_use) { + use_target_node_rotation = p_use; +} + +bool IKEffector3D::get_target_node_rotation() const { + return use_target_node_rotation; +} + +Ref IKEffector3D::get_ik_bone_3d() const { + return for_bone; +} + +bool IKEffector3D::is_following_translation_only() const { + return Math::is_zero_approx(direction_priorities.length_squared()); +} + +void IKEffector3D::set_direction_priorities(Vector3 p_direction_priorities) { + direction_priorities = p_direction_priorities; +} + +Vector3 IKEffector3D::get_direction_priorities() const { + return direction_priorities; +} + +void IKEffector3D::update_target_global_transform(Skeleton3D *p_skeleton, ManyBoneIK3D *p_many_bone_ik) { + ERR_FAIL_NULL(p_skeleton); + ERR_FAIL_NULL(for_bone); + Node3D *current_target_node = cast_to(p_many_bone_ik->get_node_or_null(target_node_path)); + if (!current_target_node || !current_target_node->is_visible_in_tree()) { + target_relative_to_skeleton_origin = p_many_bone_ik->get_godot_skeleton_transform_inverse() * for_bone->get_ik_transform()->get_global_transform(); + return; + } + target_relative_to_skeleton_origin = current_target_node->get_global_transform(); +} + +Transform3D IKEffector3D::get_target_global_transform() const { + return target_relative_to_skeleton_origin; +} + +int32_t IKEffector3D::update_effector_target_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone, const Vector *p_weights) const { + ERR_FAIL_COND_V(p_index == -1, -1); + ERR_FAIL_NULL_V(p_headings, -1); + ERR_FAIL_NULL_V(p_for_bone, -1); + ERR_FAIL_NULL_V(p_weights, -1); + + int32_t index = p_index; + Vector3 bone_origin_relative_to_skeleton_origin = for_bone->get_bone_direction_global_pose().origin; + p_headings->write[index] = target_relative_to_skeleton_origin.origin - bone_origin_relative_to_skeleton_origin; + index++; + Vector3 priority = get_direction_priorities(); + for (int axis = Vector3::AXIS_X; axis <= Vector3::AXIS_Z; ++axis) { + if (priority[axis] > 0.0) { + real_t w = p_weights->get(index); + Vector3 column = target_relative_to_skeleton_origin.basis.get_column(axis); + + p_headings->write[index] = (column + target_relative_to_skeleton_origin.origin) - bone_origin_relative_to_skeleton_origin; + p_headings->write[index] *= Vector3(w, w, w); + index++; + p_headings->write[index] = (target_relative_to_skeleton_origin.origin - column) - bone_origin_relative_to_skeleton_origin; + p_headings->write[index] *= Vector3(w, w, w); + index++; + } + } + + return index; +} + +int32_t IKEffector3D::update_effector_tip_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone) const { + ERR_FAIL_COND_V(p_index == -1, -1); + ERR_FAIL_NULL_V(p_headings, -1); + ERR_FAIL_NULL_V(p_for_bone, -1); + + Transform3D tip_xform_relative_to_skeleton_origin = for_bone->get_bone_direction_global_pose(); + Basis tip_basis = tip_xform_relative_to_skeleton_origin.basis; + Vector3 bone_origin_relative_to_skeleton_origin = p_for_bone->get_bone_direction_global_pose().origin; + + int32_t index = p_index; + p_headings->write[index] = tip_xform_relative_to_skeleton_origin.origin - bone_origin_relative_to_skeleton_origin; + index++; + double distance = target_relative_to_skeleton_origin.origin.distance_to(bone_origin_relative_to_skeleton_origin); + double scale_by = MAX(1.0, distance); + + const Vector3 priority = get_direction_priorities(); + + for (int axis = Vector3::AXIS_X; axis <= Vector3::AXIS_Z; ++axis) { + if (priority[axis] > 0.0) { + Vector3 column = tip_basis.get_column(axis) * priority[axis]; + + p_headings->write[index] = (column + tip_xform_relative_to_skeleton_origin.origin) - bone_origin_relative_to_skeleton_origin; + p_headings->write[index] *= scale_by; + index++; + + p_headings->write[index] = (tip_xform_relative_to_skeleton_origin.origin - column) - bone_origin_relative_to_skeleton_origin; + p_headings->write[index] *= scale_by; + index++; + } + } + + return index; +} + +void IKEffector3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_target_node", "skeleton", "node"), + &IKEffector3D::set_target_node); + ClassDB::bind_method(D_METHOD("get_target_node"), + &IKEffector3D::get_target_node); + ClassDB::bind_method(D_METHOD("set_passthrough_factor", "amount"), + &IKEffector3D::set_passthrough_factor); + ClassDB::bind_method(D_METHOD("get_passthrough_factor"), + &IKEffector3D::get_passthrough_factor); + + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "passthrough_factor"), "set_passthrough_factor", "get_passthrough_factor"); +} + +void IKEffector3D::set_weight(real_t p_weight) { + weight = p_weight; +} + +real_t IKEffector3D::get_weight() const { + return weight; +} + +IKEffector3D::IKEffector3D(const Ref &p_current_bone) { + ERR_FAIL_NULL(p_current_bone); + for_bone = p_current_bone; +} + +void IKEffector3D::set_passthrough_factor(float p_passthrough_factor) { + passthrough_factor = CLAMP(p_passthrough_factor, 0.0, 1.0); +} + +float IKEffector3D::get_passthrough_factor() const { + return passthrough_factor; +} diff --git a/modules/many_bone_ik/src/ik_effector_3d.h b/modules/many_bone_ik/src/ik_effector_3d.h new file mode 100644 index 000000000000..70bba8d0da83 --- /dev/null +++ b/modules/many_bone_ik/src/ik_effector_3d.h @@ -0,0 +1,90 @@ +/**************************************************************************/ +/* ik_effector_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. */ +/**************************************************************************/ + +#ifndef IK_EFFECTOR_3D_H +#define IK_EFFECTOR_3D_H + +#include "math/ik_node_3d.h" + +#include "core/object/ref_counted.h" +#include "scene/3d/skeleton_3d.h" + +#define MIN_SCALE 0.1 + +class ManyBoneIK3D; +class IKBone3D; + +class IKEffector3D : public Resource { + GDCLASS(IKEffector3D, Resource); + friend class IKBone3D; + friend class IKBoneSegment3D; + + Ref for_bone; + bool use_target_node_rotation = true; + NodePath target_node_path; + ObjectID target_node_cache; + Node *target_node_reference = nullptr; + + Transform3D target_relative_to_skeleton_origin; + int32_t num_headings = 7; + // See IKEffectorTemplate to change the defaults. + real_t weight = 0.0; + real_t passthrough_factor = 0.0; + PackedVector3Array target_headings; + PackedVector3Array tip_headings; + Vector heading_weights; + Vector3 direction_priorities; + +protected: + static void _bind_methods(); + +public: + IKEffector3D() = default; + void set_weight(real_t p_weight); + real_t get_weight() const; + void set_direction_priorities(Vector3 p_direction_priorities); + Vector3 get_direction_priorities() const; + void update_target_global_transform(Skeleton3D *p_skeleton, ManyBoneIK3D *p_modification = nullptr); + const float MAX_KUSUDAMA_LIMIT_CONES = 30; + float get_passthrough_factor() const; + void set_passthrough_factor(float p_passthrough_factor); + void set_target_node(Skeleton3D *p_skeleton, const NodePath &p_target_node_path); + NodePath get_target_node() const; + Transform3D get_target_global_transform() const; + void set_target_node_rotation(bool p_use); + bool get_target_node_rotation() const; + Ref get_ik_bone_3d() const; + bool is_following_translation_only() const; + int32_t update_effector_target_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone, const Vector *p_weights) const; + int32_t update_effector_tip_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone) const; + IKEffector3D(const Ref &p_current_bone); +}; + +#endif // IK_EFFECTOR_3D_H diff --git a/modules/many_bone_ik/src/ik_effector_template_3d.cpp b/modules/many_bone_ik/src/ik_effector_template_3d.cpp new file mode 100644 index 000000000000..d62ab47b36bc --- /dev/null +++ b/modules/many_bone_ik/src/ik_effector_template_3d.cpp @@ -0,0 +1,71 @@ +/**************************************************************************/ +/* ik_effector_template_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_effector_template_3d.h" + +#include "many_bone_ik_3d.h" + +void IKEffectorTemplate3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_target_node"), &IKEffectorTemplate3D::get_target_node); + ClassDB::bind_method(D_METHOD("set_target_node", "target_node"), &IKEffectorTemplate3D::set_target_node); + + ClassDB::bind_method(D_METHOD("get_passthrough_factor"), &IKEffectorTemplate3D::get_passthrough_factor); + ClassDB::bind_method(D_METHOD("set_passthrough_factor", "passthrough_factor"), &IKEffectorTemplate3D::set_passthrough_factor); + + ClassDB::bind_method(D_METHOD("get_weight"), &IKEffectorTemplate3D::get_weight); + ClassDB::bind_method(D_METHOD("set_weight", "weight"), &IKEffectorTemplate3D::set_weight); + + ClassDB::bind_method(D_METHOD("get_direction_priorities"), &IKEffectorTemplate3D::get_direction_priorities); + ClassDB::bind_method(D_METHOD("set_direction_priorities", "direction_priorities"), &IKEffectorTemplate3D::set_direction_priorities); + + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "passthrough_factor"), "set_passthrough_factor", "get_passthrough_factor"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "weight"), "set_weight", "get_weight"); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "direction_priorities"), "set_direction_priorities", "get_direction_priorities"); + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_node"), "set_target_node", "get_target_node"); +} + +NodePath IKEffectorTemplate3D::get_target_node() const { + return target_node; +} + +void IKEffectorTemplate3D::set_target_node(NodePath p_node_path) { + target_node = p_node_path; +} + +float IKEffectorTemplate3D::get_passthrough_factor() const { + return passthrough_factor; +} + +void IKEffectorTemplate3D::set_passthrough_factor(float p_passthrough_factor) { + passthrough_factor = p_passthrough_factor; +} + +IKEffectorTemplate3D::IKEffectorTemplate3D() { +} diff --git a/modules/many_bone_ik/src/ik_effector_template_3d.h b/modules/many_bone_ik/src/ik_effector_template_3d.h new file mode 100644 index 000000000000..c6a04a9c4390 --- /dev/null +++ b/modules/many_bone_ik/src/ik_effector_template_3d.h @@ -0,0 +1,61 @@ +/**************************************************************************/ +/* ik_effector_template_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. */ +/**************************************************************************/ + +#ifndef IK_EFFECTOR_TEMPLATE_3D_H +#define IK_EFFECTOR_TEMPLATE_3D_H + +#include "core/io/resource.h" +#include "core/object/ref_counted.h" +#include "core/string/node_path.h" + +class IKEffectorTemplate3D : public Resource { + GDCLASS(IKEffectorTemplate3D, Resource); + + NodePath target_node; + real_t passthrough_factor = 0.0f; + real_t weight = 1.0f; + Vector3 priority_direction = Vector3(0.2f, 0.0f, 0.2f); // Purported ideal values are 1.0 / 3.0 for one direction, 1.0 / 5.0 for two directions and 1.0 / 7.0 for three directions. +protected: + static void _bind_methods(); + +public: + NodePath get_target_node() const; + void set_target_node(NodePath p_node_path); + float get_passthrough_factor() const; + void set_passthrough_factor(float p_passthrough_factor); + real_t get_weight() const { return weight; } + void set_weight(real_t p_weight) { weight = p_weight; } + Vector3 get_direction_priorities() const { return priority_direction; } + void set_direction_priorities(Vector3 p_priority_direction) { priority_direction = p_priority_direction; } + + IKEffectorTemplate3D(); +}; + +#endif // IK_EFFECTOR_TEMPLATE_3D_H diff --git a/modules/many_bone_ik/src/ik_kusudama_3d.cpp b/modules/many_bone_ik/src/ik_kusudama_3d.cpp new file mode 100644 index 000000000000..db023a02ebce --- /dev/null +++ b/modules/many_bone_ik/src/ik_kusudama_3d.cpp @@ -0,0 +1,460 @@ +/**************************************************************************/ +/* ik_kusudama_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_kusudama_3d.h" + +#include "core/math/quaternion.h" +#include "ik_limit_cone_3d.h" +#include "math/ik_node_3d.h" + +void IKKusudama3D::_update_constraint() { + update_tangent_radii(); +} + +void IKKusudama3D::update_tangent_radii() { + for (int i = 0; i < limit_cones.size(); i++) { + Ref current = limit_cones[i]; + Ref next; + if (i < limit_cones.size() - 1) { + next = limit_cones[i + 1]; + } + Ref cone = limit_cones[i]; + cone->update_tangent_handles(next); + } +} + +IKKusudama3D::IKKusudama3D(Ref to_set, Ref bone_direction, Ref limiting_axes, real_t cos_half_angle_dampen) { + Vector in_bounds = { 1 }; +} + +void IKKusudama3D::set_axial_limits(real_t min_angle, real_t in_range) { + min_axial_angle = min_angle; + range_angle = in_range; + Vector3 y_axis = Vector3(0.0f, 1.0f, 0.0f); + Vector3 z_axis = Vector3(0.0f, 0.0f, 1.0f); + twist_min_rot = Quaternion(y_axis, min_axial_angle); + twist_min_vec = twist_min_rot.xform(z_axis); + twist_center_vec = twist_min_rot.xform(twist_min_vec); + twist_center_rot = Quaternion(z_axis, twist_center_vec); + twist_tan = twist_center_vec.cross(y_axis); + twist_half_range_half_cos = Math::cos(in_range / real_t(4.0)); // For the quadrance angle. We need half the range angle since starting from the center, and half of that since quadrance takes cos(angle/2). + twist_max_vec = Quaternion(y_axis, in_range).xform(twist_min_vec); + twist_max_rot = Quaternion(z_axis, twist_max_vec); + Vector3 max_cross = twist_max_vec.cross(y_axis); + flipped_bounds = twist_tan.cross(max_cross).y < real_t(0.0); + twist_min_rot.normalize(); + twist_center_rot.normalize(); + twist_max_rot.normalize(); +} + +void IKKusudama3D::set_snap_to_twist_limit(Ref bone_direction, Ref to_set, Ref constraint_axes, real_t p_dampening, real_t p_cos_half_dampen) { + if (!is_axially_constrained()) { + return; + } + + Transform3D global_transform_constraint = constraint_axes->get_global_transform(); + Quaternion global_twist_center = twist_center_rot; + + if (global_transform_constraint.basis.is_orthogonal() && Math::is_equal_approx(global_transform_constraint.basis.determinant(), real_t(1.0))) { + global_twist_center = global_transform_constraint.basis.get_rotation_quaternion() * twist_center_rot; + } + global_twist_center.normalize(); + + Transform3D global_transform_to_set = to_set->get_global_transform(); + + Quaternion align_rot = global_twist_center.inverse(); + if (global_transform_to_set.basis.is_orthogonal() && Math::is_equal_approx(global_transform_to_set.basis.determinant(), real_t(1.0))) { + align_rot = align_rot * global_transform_to_set.basis.get_rotation_quaternion(); + } + align_rot.normalize(); + + Transform3D parent_global_transform = to_set->get_parent()->get_global_transform().basis.inverse(); + Quaternion parent_global_inverse = Quaternion(); + if (parent_global_transform.basis.is_orthogonal() && Math::is_equal_approx(parent_global_transform.basis.determinant(), real_t(1.0))) { + parent_global_inverse = parent_global_transform.basis.get_rotation_quaternion(); + } + parent_global_inverse.normalize(); + + Quaternion twist_rotation, swing_rotation; // Hold the ik transform's decomposed swing and twist away from global_twist_centers's global basis. + get_swing_twist(align_rot, Vector3(0, 1, 0), swing_rotation, twist_rotation); + + if (!twist_rotation.is_equal_approx(Basis())) { + twist_rotation = IKBoneSegment3D::clamp_to_quadrance_angle(twist_rotation, twist_half_range_half_cos); + } + + Quaternion recomposition = global_twist_center * (swing_rotation * twist_rotation); + + // Ensure the dot product of the current and target quaternion is positive + if (recomposition.dot(parent_global_inverse) < 0) { + recomposition = -recomposition; + } + recomposition.normalize(); + + Quaternion rotation = parent_global_inverse * recomposition; + + Transform3D ik_transform = to_set->get_transform(); + to_set->set_transform(Transform3D(rotation, ik_transform.origin)); +} + +void IKKusudama3D::get_swing_twist( + Quaternion p_rotation, + Vector3 p_axis, + Quaternion &r_swing, + Quaternion &r_twist) { + if (p_axis.length_squared() == 0) { + return; + } + + Quaternion rotation = p_rotation; + if (rotation.w < 0.0) { + rotation *= -1; + } + + Vector3 p = p_axis * (rotation.x * p_axis.x + rotation.y * p_axis.y + rotation.z * p_axis.z); + r_twist = Quaternion(p.x, p.y, p.z, rotation.w); + + real_t d = Vector3(r_twist.x, r_twist.y, r_twist.z).dot(p_axis); + if (d < real_t(0.0)) { + r_twist *= real_t(-1.0); + } + + if (r_twist.length_squared() != 0) { + r_twist = r_twist.normalized(); + } else { + return; + } + + if (!r_twist.is_finite()) { + r_twist = Quaternion(); + } + + r_swing = rotation * r_twist.inverse(); + + if (!r_swing.is_finite()) { + r_swing = Quaternion(); + return; + } +} + +void IKKusudama3D::add_limit_cone(Vector3 new_cone_local_point, double radius) { + Ref cone = Ref(memnew(IKLimitCone3D(new_cone_local_point, radius, Ref(this)))); + limit_cones.push_back(cone); +} + +void IKKusudama3D::remove_limit_cone(Ref limitCone) { + limit_cones.erase(limitCone); +} + +real_t IKKusudama3D::get_min_axial_angle() { + return min_axial_angle; +} + +real_t IKKusudama3D::get_range_angle() { + return range_angle; +} + +bool IKKusudama3D::is_axially_constrained() { + return axially_constrained; +} + +bool IKKusudama3D::is_orientationally_constrained() { + return orientationally_constrained; +} + +void IKKusudama3D::disable_orientational_limits() { + orientationally_constrained = false; +} + +void IKKusudama3D::enable_orientational_limits() { + orientationally_constrained = true; +} + +void IKKusudama3D::toggle_orientational_limits() { + orientationally_constrained = !orientationally_constrained; +} + +void IKKusudama3D::disable_axial_limits() { + axially_constrained = false; +} + +void IKKusudama3D::enable_axial_limits() { + axially_constrained = true; +} + +void IKKusudama3D::toggle_axial_limits() { + axially_constrained = !axially_constrained; +} + +bool IKKusudama3D::is_enabled() { + return axially_constrained || orientationally_constrained; +} + +void IKKusudama3D::disable() { + axially_constrained = false; + orientationally_constrained = false; +} + +void IKKusudama3D::enable() { + axially_constrained = true; + orientationally_constrained = true; +} + +TypedArray IKKusudama3D::get_limit_cones() const { + return limit_cones; +} + +Vector3 IKKusudama3D::local_point_on_path_sequence(Vector3 in_point, Ref limiting_axes) { + double closest_point_dot = 0; + Vector3 point = limiting_axes->get_transform().xform(in_point); + point.normalize(); + Vector3 result = point; + + if (limit_cones.size() == 1) { + Ref cone = limit_cones[0]; + result = cone->get_control_point(); + } else { + for (int i = 0; i < limit_cones.size() - 1; i++) { + Ref next_cone = limit_cones[i + 1]; + Ref cone = limit_cones[i]; + Vector3 closestPathPoint = cone->get_closest_path_point(next_cone, point); + double closeDot = closestPathPoint.dot(point); + if (closeDot > closest_point_dot) { + result = closestPathPoint; + closest_point_dot = closeDot; + } + } + } + + return result; +} + +/** + * Given a point (in global coordinates), checks to see if a ray can be extended from the Kusudama's + * origin to that point, such that the ray in the Kusudama's reference frame is within the range_angle allowed by the Kusudama's + * coneLimits. + * If such a ray exists, the original point is returned (the point is within the limits). + * If it cannot exist, the tip of the ray within the kusudama's limits that would require the least rotation + * to arrive at the input point is returned. + * @param in_point the point to test. + * @param in_bounds returns a number from -1 to 1 representing the point's distance from the boundary, 0 means the point is right on + * the boundary, 1 means the point is within the boundary and on the path furthest from the boundary. any negative number means + * the point is outside of the boundary, but does not signify anything about how far from the boundary the point is. + * @return the original point, if it's in limits, or the closest point which is in limits. + */ +Vector3 IKKusudama3D::get_local_point_in_limits(Vector3 in_point, Vector *in_bounds) { + Vector3 point = in_point.normalized(); + real_t closest_cos = -2.0; + in_bounds->write[0] = -1; + Vector3 closest_collision_point = Vector3(NAN, NAN, NAN); + // This is an exact check for being inside the bounds of each individual cone. + for (int i = 0; i < limit_cones.size(); i++) { + Ref cone = limit_cones[i]; + Vector3 collision_point = cone->closest_to_cone(point, in_bounds); + if (Math::is_nan(collision_point.x) || Math::is_nan(collision_point.y) || Math::is_nan(collision_point.z)) { + in_bounds->write[0] = 1; + return point; + } + real_t this_cos = collision_point.dot(point); + if (Math::is_nan(closest_collision_point.x) || Math::is_nan(closest_collision_point.y) || Math::is_nan(closest_collision_point.z) || this_cos > closest_cos) { + closest_collision_point = collision_point; + closest_cos = this_cos; + } + } + if ((*in_bounds)[0] == -1) { + // Case where there are multiple cones and we're out of bounds of all cones. + // Are we in the paths between the cones. + for (int i = 0; i < limit_cones.size() - 1; i++) { + Ref currCone = limit_cones[i]; + Ref nextCone = limit_cones[i + 1]; + Vector3 collision_point = currCone->get_on_great_tangent_triangle(nextCone, point); + if (Math::is_nan(collision_point.x)) { + continue; + } + real_t this_cos = collision_point.dot(point); + if (Math::is_equal_approx(this_cos, real_t(1.0))) { + in_bounds->write[0] = 1; + return point; + } + if (this_cos > closest_cos) { + closest_collision_point = collision_point; + closest_cos = this_cos; + } + } + } + // Return the closest boundary point between cones. + return closest_collision_point; +} + +void IKKusudama3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_limit_cones"), &IKKusudama3D::get_limit_cones); + ClassDB::bind_method(D_METHOD("set_limit_cones", "limit_cones"), &IKKusudama3D::set_limit_cones); +} + +void IKKusudama3D::set_limit_cones(TypedArray p_cones) { + limit_cones = p_cones; +} + +void IKKusudama3D::set_axes_to_orientation_snap(Ref bone_direction, Ref to_set, Ref limiting_axes, real_t p_dampening, real_t p_cos_half_angle_dampen) { + if (bone_direction.is_null()) { + return; + } + if (to_set.is_null()) { + return; + } + if (limiting_axes.is_null()) { + return; + } + Vector in_bounds; + in_bounds.resize(1); + in_bounds.write[0] = 1.0; + Vector3 limiting_origin = limiting_axes->get_global_transform().origin; + Vector3 bone_dir_xform = bone_direction->get_global_transform().xform(Vector3(0.0, 1.0, 0.0)); + + bone_ray->set_point_1(limiting_origin); + bone_ray->set_point_2(bone_dir_xform); + + Vector3 bone_tip = limiting_axes->to_local(bone_ray->get_point_2()); + Vector3 in_limits = get_local_point_in_limits(bone_tip, &in_bounds); + + if (in_bounds[0] < 0 && !is_nan_vector(in_limits)) { + constrained_ray->set_point_1(bone_ray->get_point_1()); + constrained_ray->set_point_2(limiting_axes->to_global(in_limits)); + + Quaternion rectified_rot = Quaternion(bone_ray->get_heading(), constrained_ray->get_heading()); + to_set->rotate_local_with_global(rectified_rot); + } +} + +void IKKusudama3D::set_axes_to_returnfulled(Ref bone_direction, Ref to_set, Ref limiting_axes, float cos_half_returnfullness, float angle_returnfullness) { + if (bone_direction.is_null() || to_set.is_null() || limiting_axes.is_null() || resistance <= 0.0) { + return; + } + Quaternion twist_rotation, swing_rotation; + get_swing_twist(bone_direction->get_global_transform().basis, Vector3(0, 1, 0), swing_rotation, twist_rotation); + if (orientationally_constrained) { + Vector3 origin = bone_direction->get_global_transform().origin; + Vector3 limiting_origin = limiting_axes->get_global_transform().origin; + Vector3 bone_dir_xform = bone_direction->get_global_transform().xform(Vector3(0.0, 1.0, 0.0)); + + bone_ray->set_point_1(limiting_origin); + bone_ray->set_point_2(bone_dir_xform); + + Vector3 in_point = bone_ray->get_point_2(); + Vector3 path_point = local_point_on_path_sequence(in_point, limiting_axes); + in_point -= origin; + path_point -= origin; + + Quaternion to_clamp = Quaternion(in_point, path_point); + to_clamp = clamp_to_quadrance_angle(to_clamp, cos_half_returnfullness).normalized(); + to_set->rotate_local_with_global(to_clamp); + } + + if (axially_constrained) { + double angle_to_twist_mid = angle_to_twist_center(bone_direction, limiting_axes); + double clamped_angle = CLAMP(angle_to_twist_mid, -angle_returnfullness, angle_returnfullness); + Vector3 bone_axis_y = bone_direction->get_global_transform().xform(Vector3(0, 1, 0)); + Quaternion rotation = Quaternion(bone_axis_y, clamped_angle).normalized(); + to_set->rotate_local_with_global(rotation, false); + } +} + +double IKKusudama3D::angle_to_twist_center(Ref bone_direction, Ref limiting_axes) { + if (!is_axially_constrained()) { + return 0; + } + Quaternion inverse_rotation = limiting_axes->get_global_transform().basis.get_rotation_quaternion().inverse(); + Quaternion align_rotation = inverse_rotation * bone_direction->get_global_transform().basis.get_rotation_quaternion(); + Vector3 twisted_direction = align_rotation.xform(Vector3(0, 0, 1)); + Quaternion to_mid = Quaternion(twisted_direction, twist_center_vec); + return to_mid.get_angle() * to_mid.get_axis().y; +} + +bool IKKusudama3D::is_nan_vector(const Vector3 &vec) { + return Math::is_nan(vec.x) || Math::is_nan(vec.y) || Math::is_nan(vec.z); +} + +void IKKusudama3D::set_resistance(float p_resistance) { + resistance = p_resistance; +} + +float IKKusudama3D::get_resistance() { + return resistance; +} + +Quaternion IKKusudama3D::clamp_to_quadrance_angle(Quaternion p_rotation, double p_cos_half_angle) { + Quaternion rotation = p_rotation; + double newCoeff = 1.0 - (p_cos_half_angle * abs(p_cos_half_angle)); + double currentCoeff = rotation.x * rotation.x + rotation.y * rotation.y + rotation.z * rotation.z; + if (newCoeff >= currentCoeff) { + return rotation; + } + double over_limit = (currentCoeff - newCoeff) / (1.0 - newCoeff); + Quaternion clamped_rotation = rotation; + clamped_rotation.w = rotation.w < 0 ? -p_cos_half_angle : p_cos_half_angle; + double compositeCoeff = sqrt(newCoeff / currentCoeff); + clamped_rotation.x *= compositeCoeff; + clamped_rotation.y *= compositeCoeff; + clamped_rotation.z *= compositeCoeff; + if (!rotation.is_finite()) { + return Quaternion(); + } + return rotation.slerp(clamped_rotation, over_limit); +} + +void IKKusudama3D::set_current_twist_rotation(Ref p_godot_skeleton_aligned_transform, Ref p_bone_direction, Ref p_twist_transform, real_t p_rotation) { + p_rotation = 1 / Math_TAU * p_rotation; + Quaternion align_rot_inv = p_twist_transform->get_global_transform().basis.inverse().get_rotation_quaternion(); + Quaternion align_rot = align_rot_inv * p_bone_direction->get_global_transform().basis.get_rotation_quaternion(); + Quaternion twist_rotation, swing_rotation; + get_swing_twist(align_rot, Vector3(0, 1, 0), swing_rotation, twist_rotation); + Vector3 axis = (twist_max_vec - twist_min_vec).normalized(); + twist_rotation = Quaternion(axis, p_rotation); + Quaternion recomposition = swing_rotation * twist_rotation; + Quaternion parent_global_inverse = p_godot_skeleton_aligned_transform->get_parent()->get_global_transform().basis.inverse().get_rotation_quaternion(); + Basis rotation_basis = parent_global_inverse * (recomposition); + Transform3D ik_transform = p_godot_skeleton_aligned_transform->get_transform(); + p_godot_skeleton_aligned_transform->set_transform(Transform3D(rotation_basis, ik_transform.origin)); +} + +real_t IKKusudama3D::get_current_twist_rotation(Ref p_godot_skeleton_aligned_transform, Ref p_bone_direction, Ref p_twist_transform) { + Quaternion global_twist_center = p_twist_transform->get_global_transform().basis.inverse().get_rotation_quaternion() * twist_center_rot; + Quaternion align_rot = global_twist_center * p_bone_direction->get_global_transform().basis.get_rotation_quaternion(); + Quaternion twist_rotation, swing_rotation; + get_swing_twist(align_rot, Vector3(0, 1, 0), swing_rotation, twist_rotation); + if (range_angle == 0.0) { + return 0; + } + Vector3 z_axis = Vector3(0.0f, 0.0f, 1.0f); + Vector3 twist_vec = twist_rotation.xform(z_axis); + real_t min_angle = twist_vec.angle_to(twist_min_vec); + real_t max_angle = twist_vec.angle_to(twist_max_vec); + real_t center_angle = (min_angle + max_angle) / 2.0; + return center_angle / Math_TAU; +} diff --git a/modules/many_bone_ik/src/ik_kusudama_3d.h b/modules/many_bone_ik/src/ik_kusudama_3d.h new file mode 100644 index 000000000000..b83b84755c6d --- /dev/null +++ b/modules/many_bone_ik/src/ik_kusudama_3d.h @@ -0,0 +1,221 @@ +/**************************************************************************/ +/* ik_kusudama_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. */ +/**************************************************************************/ + +#ifndef IK_KUSUDAMA_3D_H +#define IK_KUSUDAMA_3D_H + +#include "ik_bone_3d.h" +#include "ik_bone_segment_3d.h" +#include "ik_limit_cone_3d.h" +#include "ik_ray_3d.h" +#include "math/ik_node_3d.h" + +#include "core/io/resource.h" +#include "core/math/quaternion.h" +#include "core/object/ref_counted.h" +#include "core/variant/typed_array.h" +#include "scene/3d/node_3d.h" + +class IKBone3D; +class IKLimitCone3D; +class IKKusudama3D : public Resource { + GDCLASS(IKKusudama3D, Resource); + + /** + * An array containing all of the Kusudama's limit_cones. The kusudama is built up + * with the expectation that any limitCone in the array is connected to the cone at the previous element in the array, + * and the cone at the next element in the array. + */ + TypedArray limit_cones; + + Quaternion twist_min_rot; + Vector3 twist_min_vec; + Vector3 twist_max_vec; + Vector3 twist_center_vec; + Quaternion twist_center_rot; + Quaternion twist_max_rot; + real_t twist_half_range_half_cos = 0; + Vector3 twist_tan; + bool flipped_bounds = false; + real_t resistance = 0; + + /** + * Defined as some Angle in radians about the limiting_axes Y axis, 0 being equivalent to the + * limiting_axes Z axis. + */ + real_t min_axial_angle = 0.0; + /** + * Defined as some Angle in radians about the limiting_axes Y axis, 0 being equivalent to the + * min_axial_angle + */ + real_t range_angle = Math_TAU; + + bool orientationally_constrained = false; + bool axially_constrained = false; + +protected: + static void _bind_methods(); + +public: + ~IKKusudama3D() { + } + + IKKusudama3D() {} + + IKKusudama3D(Ref to_set, Ref bone_direction, Ref limiting_axes, real_t cos_half_angle_dampen); + + void _update_constraint(); + + void update_tangent_radii(); + + Ref bone_ray = Ref(memnew(IKRay3D())); + Ref constrained_ray = Ref(memnew(IKRay3D())); + double unit_hyper_area = 2 * Math::pow(Math_PI, 2); + double unit_area = 4 * Math_PI; + + /** + * Get the swing rotation and twist rotation for the specified axis. The twist rotation represents the rotation around the specified axis. The swing rotation represents the rotation of the specified + * axis itself, which is the rotation around an axis perpendicular to the specified axis. The swing and twist rotation can be + * used to reconstruct the original quaternion: this = swing * twist + * + * @param p_axis the X, Y, Z component of the normalized axis for which to get the swing and twist rotation + * @return twist represent the rotational twist + * @return swing represent the rotational swing + * @see calculation + */ + static void get_swing_twist( + Quaternion p_rotation, + Vector3 p_axis, + Quaternion &r_swing, + Quaternion &r_twist); + +public: + real_t get_current_twist_rotation(Ref p_godot_skeleton_aligned_transform, Ref p_bone_direction, Ref p_twist_transform); + void set_current_twist_rotation(Ref p_godot_skeleton_aligned_transform, Ref p_bone_direction, Ref p_twist_transform, real_t p_rotation); + double angle_to_twist_center(Ref bone_direction, Ref limiting_axes); + /** + * Presumes the input axes are the bone's localAxes, and rotates + * them to satisfy the snap limits. + * + * @param to_set + */ + void set_axes_to_orientation_snap(Ref bone_direction, Ref to_set, Ref limiting_axes, real_t p_dampening, real_t p_cos_half_angle_dampen); + + bool is_nan_vector(const Vector3 &vec); + + /** + * Kusudama constraints decompose the bone orientation into a swing component, and a twist component. + * The "Swing" component is the final direction of the bone. The "Twist" component represents how much + * the bone is rotated about its own final direction. Where limit cones allow you to constrain the "Swing" + * component, this method lets you constrain the "twist" component. + * + * @param min_angle some angle in radians about the major rotation frame's y-axis to serve as the first angle within the range_angle that the bone is allowed to twist. + * @param in_range some angle in radians added to the min_angle. if the bone's local Z goes maxAngle radians beyond the min_angle, it is considered past the limit. + * This value is always interpreted as being in the positive direction. For example, if this value is -PI/2, the entire range_angle from min_angle to min_angle + 3PI/4 is + * considered valid. + */ + void set_axial_limits(real_t min_angle, real_t in_range); + + /** + * + * @param to_set + * @param limiting_axes + * @return radians of the twist required to snap bone into twist limits (0 if bone is already in twist limits) + */ + void set_snap_to_twist_limit(Ref bone_direction, Ref to_set, Ref limiting_axes, real_t p_dampening, real_t p_cos_half_dampen); + + /** + * Given a point (in local coordinates), checks to see if a ray can be extended from the Kusudama's + * origin to that point, such that the ray in the Kusudama's reference frame is within the range_angle allowed by the Kusudama's + * coneLimits. + * If such a ray exists, the original point is returned (the point is within the limits). + * If it cannot exist, the tip of the ray within the kusudama's limits that would require the least rotation + * to arrive at the input point is returned. + * @param in_point the point to test. + * @param in_bounds should be an array with at least 2 elements. The first element will be set to a number from -1 to 1 representing the point's distance from the boundary, 0 means the point is right on + * the boundary, 1 means the point is within the boundary and on the path furthest from the boundary. any negative number means + * the point is outside of the boundary, but does not signify anything about how far from the boundary the point is. + * The second element will be given a value corresponding to the limit cone whose bounds were exceeded. If the bounds were exceeded on a segment between two limit cones, + * this value will be set to a non-integer value between the two indices of the limitcone comprising the segment whose bounds were exceeded. + * @return the original point, if it's in limits, or the closest point which is in limits. + */ + Vector3 get_local_point_in_limits(Vector3 in_point, Vector *in_bounds); + + Vector3 local_point_on_path_sequence(Vector3 in_point, Ref limiting_axes); + + /** + * Add a IKLimitCone to the Kusudama. + * @param new_point where on the Kusudama to add the LimitCone (in Kusudama's local coordinate frame defined by its bone's majorRotationAxes)) + * @param radius the radius of the limitCone + */ + void add_limit_cone(Vector3 new_point, double radius); + void remove_limit_cone(Ref limitCone); + + /** + * + * @return the lower bound on the axial constraint + */ + real_t get_min_axial_angle(); + real_t get_range_angle(); + + bool is_axially_constrained(); + bool is_orientationally_constrained(); + void disable_orientational_limits(); + void enable_orientational_limits(); + void toggle_orientational_limits(); + void disable_axial_limits(); + void enable_axial_limits(); + void toggle_axial_limits(); + bool is_enabled(); + void disable(); + void enable(); + + /** + * TODO: This functionality is not yet fully implemented It always returns an overly simplistic representation + * not in line with what is described below. + * + * @return an (approximate) measure of the amount of rotational + * freedom afforded by this kusudama, with 0 meaning no rotational + * freedom, and 1 meaning total unconstrained freedom. + * + * This is approximately computed as a ratio between the orientations the bone can be in + * vs the orientations it cannot be in. Note that unfortunately this function double counts + * the orientations a bone can be in between two limit cones in a sequence if those limit + * cones intersect with a previous sequence. + */ + TypedArray get_limit_cones() const; + void set_limit_cones(TypedArray p_cones); + float get_resistance(); + void set_resistance(float p_resistance); + static Quaternion clamp_to_quadrance_angle(Quaternion p_rotation, double p_cos_half_angle); + void set_axes_to_returnfulled(Ref bone_direction, Ref to_set, Ref limiting_axes, float cos_half_returnfullness, float angle_returnfullness); +}; + +#endif // IK_KUSUDAMA_3D_H diff --git a/modules/many_bone_ik/src/ik_limit_cone_3d.cpp b/modules/many_bone_ik/src/ik_limit_cone_3d.cpp new file mode 100644 index 000000000000..bb86abff6828 --- /dev/null +++ b/modules/many_bone_ik/src/ik_limit_cone_3d.cpp @@ -0,0 +1,406 @@ +/**************************************************************************/ +/* ik_limit_cone_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_limit_cone_3d.h" + +#include "core/error/error_macros.h" +#include "core/math/quaternion.h" +#include "ik_kusudama_3d.h" + +#include "core/io/resource.h" +#include "core/object/ref_counted.h" + +void IKLimitCone3D::update_tangent_handles(Ref p_next) { + if (p_next.is_valid()) { + double radA = _get_radius(); + double radB = p_next->_get_radius(); + + Vector3 A = get_control_point(); + Vector3 B = p_next->get_control_point(); + + Vector3 arc_normal = A.cross(B).normalized(); + + /** + * There are an infinite number of circles co-tangent with A and B, every other + * one of which has a unique radius. + * + * However, we want the radius of our tangent circles to obey the following properties: + * 1) When the radius of A + B == 0, our tangent circle's radius should = 90. + * In other words, the tangent circle should span a hemisphere. + * 2) When the radius of A + B == 180, our tangent circle's radius should = 0. + * In other words, when A + B combined are capable of spanning the entire sphere, + * our tangentCircle should be nothing. + * + * Another way to think of this is -- whatever the maximum distance can be between the + * borders of A and B (presuming their centers are free to move about the circle + * but their radii remain constant), we want our tangentCircle's diameter to be precisely that distance, + * and so, our tangent circles radius should be precisely half of that distance. + */ + double tRadius = (Math_PI - (radA + radB)) / 2; + + /** + * Once we have the desired radius for our tangent circle, we may find the solution for its + * centers (usually, there are two). + */ + double boundaryPlusTangentRadiusA = radA + tRadius; + double boundaryPlusTangentRadiusB = radB + tRadius; + + // the axis of this cone, scaled to minimize its distance to the tangent contact points. + Vector3 scaledAxisA = A * Math::cos(boundaryPlusTangentRadiusA); + // a point on the plane running through the tangent contact points + Quaternion temp_var = Quaternion(arc_normal, boundaryPlusTangentRadiusA); + Vector3 planeDir1A = temp_var.xform(A); + // another point on the same plane + Quaternion tempVar2 = Quaternion(A, Math_PI / 2); + Vector3 planeDir2A = tempVar2.xform(planeDir1A); + + Vector3 scaledAxisB = B * cos(boundaryPlusTangentRadiusB); + // a point on the plane running through the tangent contact points + Quaternion tempVar3 = Quaternion(arc_normal, boundaryPlusTangentRadiusB); + Vector3 planeDir1B = tempVar3.xform(B); + // another point on the same plane + Quaternion tempVar4 = Quaternion(B, Math_PI / 2); + Vector3 planeDir2B = tempVar4.xform(planeDir1B); + + // ray from scaled center of next cone to half way point between the circumference of this cone and the next cone. + Ref r1B = Ref(memnew(IKRay3D(planeDir1B, scaledAxisB))); + Ref r2B = Ref(memnew(IKRay3D(planeDir1B, planeDir2B))); + + r1B->elongate(99); + r2B->elongate(99); + + Vector3 intersection1 = r1B->get_intersects_plane(scaledAxisA, planeDir1A, planeDir2A); + Vector3 intersection2 = r2B->get_intersects_plane(scaledAxisA, planeDir1A, planeDir2A); + + Ref intersectionRay = Ref(memnew(IKRay3D(intersection1, intersection2))); + intersectionRay->elongate(99); + + Vector3 sphereIntersect1; + Vector3 sphereIntersect2; + Vector3 sphereCenter; + intersectionRay->intersects_sphere(sphereCenter, 1.0f, &sphereIntersect1, &sphereIntersect2); + + set_tangent_circle_center_next_1(sphereIntersect1); + set_tangent_circle_center_next_2(sphereIntersect2); + set_tangent_circle_radius_next(tRadius); + } + if (tangent_circle_center_next_1 == Vector3(NAN, NAN, NAN)) { + tangent_circle_center_next_1 = get_orthogonal(control_point).normalized(); + } + if (tangent_circle_center_next_2 == Vector3(NAN, NAN, NAN)) { + tangent_circle_center_next_2 = (tangent_circle_center_next_1 * -1).normalized(); + } + if (p_next.is_valid()) { + compute_triangles(p_next); + } +} + +void IKLimitCone3D::set_tangent_circle_radius_next(double rad) { + tangent_circle_radius_next = rad; + tangent_circle_radius_next_cos = cos(tangent_circle_radius_next); +} + +Vector3 IKLimitCone3D::get_tangent_circle_center_next_1() { + return tangent_circle_center_next_1; +} + +double IKLimitCone3D::get_tangent_circle_radius_next() { + return tangent_circle_radius_next; +} + +double IKLimitCone3D::get_tangent_circle_radius_next_cos() { + return tangent_circle_radius_next_cos; +} + +Vector3 IKLimitCone3D::get_tangent_circle_center_next_2() { + return tangent_circle_center_next_2; +} + +double IKLimitCone3D::_get_radius() { + return radius; +} + +double IKLimitCone3D::_get_radius_cosine() { + return radius_cosine; +} + +void IKLimitCone3D::compute_triangles(Ref p_next) { + if (p_next.is_null()) { + return; + } + first_triangle_next.write[1] = tangent_circle_center_next_1.normalized(); + first_triangle_next.write[0] = get_control_point().normalized(); + first_triangle_next.write[2] = p_next->get_control_point().normalized(); + + second_triangle_next.write[1] = tangent_circle_center_next_2.normalized(); + second_triangle_next.write[0] = get_control_point().normalized(); + second_triangle_next.write[2] = p_next->get_control_point().normalized(); +} + +Vector3 IKLimitCone3D::get_control_point() const { + return control_point; +} + +void IKLimitCone3D::set_control_point(Vector3 p_control_point) { + control_point = p_control_point; + control_point.normalize(); +} + +double IKLimitCone3D::get_radius() const { + return radius; +} + +double IKLimitCone3D::get_radius_cosine() const { + return radius_cosine; +} + +void IKLimitCone3D::set_radius(double p_radius) { + radius = p_radius; + radius_cosine = cos(p_radius); +} + +bool IKLimitCone3D::determine_if_in_bounds(Ref next, Vector3 input) const { + /** + * Procedure : Check if input is contained in this cone, or the next cone + * if it is, then we're finished and in bounds. otherwise, + * check if the point is contained within the tangent radii, + * if it is, then we're out of bounds and finished, otherwise + * in the tangent triangles while still remaining outside of the tangent radii + * if it is, then we're finished and in bounds. otherwise, we're out of bounds. + */ + + if (control_point.dot(input) >= radius_cosine) { + return true; + } else if (next != nullptr && next->control_point.dot(input) >= next->radius_cosine) { + return true; + } else { + if (next == nullptr) { + return false; + } + bool inTan1Rad = tangent_circle_center_next_1.dot(input) > tangent_circle_radius_next_cos; + if (inTan1Rad) { + return false; + } + bool inTan2Rad = tangent_circle_center_next_2.dot(input) > tangent_circle_radius_next_cos; + if (inTan2Rad) { + return false; + } + + /*if we reach this point in the code, we are either on the path between two limit_cones, or on the path extending out from between them + * but outside of their radii. + * To determine which , we take the cross product of each control point with each tangent center. + * The direction of each of the resultant vectors will represent the normal of a plane. + * Each of these four planes define part of a boundary which determines if our point is in bounds. + * If the dot product of our point with the normal of any of these planes is negative, we must be out + * of bounds. + * + * Older version of this code relied on a triangle intersection algorithm here, which I think is slightly less efficient on average + * as it didn't allow for early termination. . + */ + + Vector3 c1xc2 = control_point.cross(next->control_point); + double c1c2dir = input.dot(c1xc2); + + if (c1c2dir < 0.0) { + Vector3 c1xt1 = control_point.cross(tangent_circle_center_next_1); + Vector3 t1xc2 = tangent_circle_center_next_1.cross(next->control_point); + return input.dot(c1xt1) > 0 && input.dot(t1xc2) > 0; + } else { + Vector3 t2xc1 = tangent_circle_center_next_2.cross(control_point); + Vector3 c2xt2 = next->control_point.cross(tangent_circle_center_next_2); + return input.dot(t2xc1) > 0 && input.dot(c2xt2) > 0; + } + } +} + +Vector3 IKLimitCone3D::get_closest_path_point(Ref next, Vector3 input) const { + Vector3 result = get_on_path_sequence(next, input); + bool is_number = !(Math::is_nan(result.x) && Math::is_nan(result.y) && Math::is_nan(result.z)); + if (!is_number) { + result = closest_cone(next, input); + } + return result; +} + +Vector3 IKLimitCone3D::get_closest_collision(Ref next, Vector3 input) const { + Vector3 result = get_on_great_tangent_triangle(next, input); + + bool is_number = !(Math::is_nan(result.x) && Math::is_nan(result.y) && Math::is_nan(result.z)); + if (!is_number) { + Vector in_bounds = { 0.0 }; + result = closest_point_on_closest_cone(next, input, &in_bounds); + } + return result; +} + +Vector3 IKLimitCone3D::get_orthogonal(Vector3 p_in) { + Vector3 result; + float threshold = p_in.length() * 0.6f; + if (threshold > 0.f) { + if (Math::abs(p_in.x) <= threshold) { + float inverse = 1.f / Math::sqrt(p_in.y * p_in.y + p_in.z * p_in.z); + return result = Vector3(0.f, inverse * p_in.z, -inverse * p_in.y); + } else if (Math::abs(p_in.y) <= threshold) { + float inverse = 1.f / Math::sqrt(p_in.x * p_in.x + p_in.z * p_in.z); + return result = Vector3(-inverse * p_in.z, 0.f, inverse * p_in.x); + } + float inverse = 1.f / Math::sqrt(p_in.x * p_in.x + p_in.y * p_in.y); + return result = Vector3(inverse * p_in.y, -inverse * p_in.x, 0.f); + } + + return result; +} + +IKLimitCone3D::IKLimitCone3D(Vector3 direction, double rad, Ref attached_to) { + parent_kusudama = attached_to; + tangent_circle_center_next_1 = Vector3(0.0f, -1.0f, 0.0f); + tangent_circle_center_next_2 = Vector3(0.0f, 1.0f, 0.0f); + set_radius(MAX(1.0e-38, rad)); + control_point = direction.normalized(); +} + +Vector3 IKLimitCone3D::get_on_great_tangent_triangle(Ref next, Vector3 input) const { + Vector3 c1xc2 = control_point.cross(next->control_point); + double c1c2dir = input.dot(c1xc2); + if (c1c2dir < 0.0) { + Vector3 c1xt1 = control_point.cross(tangent_circle_center_next_1).normalized(); + Vector3 t1xc2 = tangent_circle_center_next_1.cross(next->control_point).normalized(); + if (input.dot(c1xt1) > 0 && input.dot(t1xc2) > 0) { + double to_next_cos = input.dot(tangent_circle_center_next_1); + if (to_next_cos > tangent_circle_radius_next_cos) { + Vector3 plane_normal = tangent_circle_center_next_1.cross(input).normalized(); + plane_normal.normalize(); + Quaternion rotate_about_by = Quaternion(plane_normal, tangent_circle_radius_next); + return rotate_about_by.xform(tangent_circle_center_next_1); + } else { + return input; + } + } else { + return Vector3(NAN, NAN, NAN); + } + } else { + Vector3 t2xc1 = tangent_circle_center_next_2.cross(control_point).normalized(); + Vector3 c2xt2 = next->control_point.cross(tangent_circle_center_next_2).normalized(); + if (input.dot(t2xc1) > 0 && input.dot(c2xt2) > 0) { + if (input.dot(tangent_circle_center_next_2) > tangent_circle_radius_next_cos) { + Vector3 plane_normal = tangent_circle_center_next_2.cross(input).normalized(); + plane_normal.normalize(); + Quaternion rotate_about_by = Quaternion(plane_normal, tangent_circle_radius_next); + return rotate_about_by.xform(tangent_circle_center_next_2); + } else { + return input; + } + } else { + return Vector3(NAN, NAN, NAN); + } + } +} + +Vector3 IKLimitCone3D::closest_cone(Ref next, Vector3 input) const { + if (input.dot(control_point) > input.dot(next->control_point)) { + return control_point; + } else { + return next->control_point; + } +} + +Vector3 IKLimitCone3D::closest_point_on_closest_cone(Ref next, Vector3 input, Vector *in_bounds) const { + Vector3 closestToFirst = closest_to_cone(input, in_bounds); + if ((*in_bounds)[0] > 0.0) { + return closestToFirst; + } + Vector3 closestToSecond = next->closest_to_cone(input, in_bounds); + if ((*in_bounds)[0] > 0.0) { + return closestToSecond; + } + double cosToFirst = input.dot(closestToFirst); + double cosToSecond = input.dot(closestToSecond); + + if (cosToFirst > cosToSecond) { + return closestToFirst; + } else { + return closestToSecond; + } +} + +Vector3 IKLimitCone3D::closest_to_cone(Vector3 input, Vector *in_bounds) const { + Vector3 normalized_input = input.normalized(); + Vector3 normalized_control_point = get_control_point().normalized(); + if (normalized_input.dot(normalized_control_point) > get_radius_cosine()) { + in_bounds->write[0] = 1.0; + return Vector3(NAN, NAN, NAN); + } + Vector3 axis = normalized_control_point.cross(normalized_input).normalized(); + if (Math::is_zero_approx(axis.length_squared()) || !axis.is_finite()) { + axis = Vector3(0, 1, 0); + } + Quaternion rot_to = Quaternion(axis, get_radius()); + Vector3 axis_control_point = normalized_control_point; + if (Math::is_zero_approx(axis_control_point.length_squared())) { + axis_control_point = Vector3(0, 1, 0); + } + Vector3 result = rot_to.xform(axis_control_point); + in_bounds->write[0] = -1; + return result; +} + +void IKLimitCone3D::set_tangent_circle_center_next_1(Vector3 point) { + tangent_circle_center_next_1 = point.normalized(); +} + +void IKLimitCone3D::set_tangent_circle_center_next_2(Vector3 point) { + tangent_circle_center_next_2 = point.normalized(); +} + +Vector3 IKLimitCone3D::get_on_path_sequence(Ref next, Vector3 input) const { + Vector3 c1xc2 = get_control_point().cross(next->control_point).normalized(); + double c1c2dir = input.dot(c1xc2); + if (c1c2dir < 0.0) { + Vector3 c1xt1 = get_control_point().cross(tangent_circle_center_next_1).normalized(); + Vector3 t1xc2 = tangent_circle_center_next_1.cross(next->get_control_point()).normalized(); + if (input.dot(c1xt1) > 0.0f && input.dot(t1xc2) > 0.0f) { + Ref tan1ToInput = Ref(memnew(IKRay3D(tangent_circle_center_next_1, input))); + Vector3 result = tan1ToInput->get_intersects_plane(Vector3(0.0f, 0.0f, 0.0f), get_control_point(), next->get_control_point()); + return result.normalized(); + } else { + return Vector3(NAN, NAN, NAN); + } + } else { + Vector3 t2xc1 = tangent_circle_center_next_2.cross(control_point).normalized(); + Vector3 c2xt2 = next->get_control_point().cross(tangent_circle_center_next_2).normalized(); + if (input.dot(t2xc1) > 0 && input.dot(c2xt2) > 0) { + Ref tan2ToInput = Ref(memnew(IKRay3D(tangent_circle_center_next_2, input))); + Vector3 result = tan2ToInput->get_intersects_plane(Vector3(0.0f, 0.0f, 0.0f), get_control_point(), next->get_control_point()); + return result.normalized(); + } else { + return Vector3(NAN, NAN, NAN); + } + } +} diff --git a/modules/many_bone_ik/src/ik_limit_cone_3d.h b/modules/many_bone_ik/src/ik_limit_cone_3d.h new file mode 100644 index 000000000000..84f538ce9ea6 --- /dev/null +++ b/modules/many_bone_ik/src/ik_limit_cone_3d.h @@ -0,0 +1,142 @@ +/**************************************************************************/ +/* ik_limit_cone_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. */ +/**************************************************************************/ + +#ifndef IK_LIMIT_CONE_3D_H +#define IK_LIMIT_CONE_3D_H + +#include "ik_bone_3d.h" +#include "ik_ray_3d.h" + +#include "core/io/resource.h" +#include "core/math/vector3.h" +#include "core/object/ref_counted.h" + +class IKKusudama3D; +class IKLimitCone3D : public Resource { + GDCLASS(IKLimitCone3D, Resource); + void compute_triangles(Ref p_next); + + Vector3 control_point; + Vector3 radial_point; + + // Radius stored as cosine to save on the acos call necessary for the angle between. + double radius_cosine = 0; + double radius = 0; + Vector3 closest_cone(Ref next, Vector3 input) const; + void set_tangent_circle_radius_next(double rad); + Ref parent_kusudama; + + Vector3 tangent_circle_center_next_1; + Vector3 tangent_circle_center_next_2; + double tangent_circle_radius_next = 0; + double tangent_circle_radius_next_cos = 0; + + /** + * A triangle where the [1] is the tangent_circle_next_n, and [0] and [2] + * are the points at which the tangent circle intersects this IKLimitCone and the + * next IKLimitCone. + */ + Vector first_triangle_next = { Vector3(), Vector3(), Vector3() }; + Vector second_triangle_next = { Vector3(), Vector3(), Vector3() }; + + /** + * + * @param next + * @param input + * @return null if the input point is already in bounds, or the point's rectified position + * if the point was out of bounds. + */ + Vector3 get_closest_collision(Ref next, Vector3 input) const; + + /** + * Determines if a ray emanating from the origin to given point in local space + * lies within the path from this cone to the next cone. This function relies on + * an optimization trick for a performance boost, but the trick ruins everything + * if the input isn't normalized. So it is ABSOLUTELY VITAL + * that @param input have unit length in order for this function to work correctly. + * @param next + * @param input + * @return + */ + bool determine_if_in_bounds(Ref next, Vector3 input) const; + Vector3 get_on_path_sequence(Ref next, Vector3 input) const; + + /** + * returns null if no rectification is required. + * @param next + * @param input + * @param in_bounds + * @return + */ + Vector3 closest_point_on_closest_cone(Ref next, Vector3 input, Vector *in_bounds) const; + + double get_tangent_circle_radius_next_cos(); + static Vector3 get_orthogonal(Vector3 p_in); + +protected: + double _get_radius(); + + double _get_radius_cosine(); + +public: + IKLimitCone3D() {} + virtual ~IKLimitCone3D() {} + IKLimitCone3D(Vector3 direction, double rad, Ref attached_to); + void update_tangent_handles(Ref p_next); + void set_tangent_circle_center_next_1(Vector3 point); + void set_tangent_circle_center_next_2(Vector3 point); + /** + * + * @param next + * @param input + * @return null if inapplicable for rectification. the original point if in bounds, or the point rectified to the closest boundary on the path sequence + * between two cones if the point is out of bounds and applicable for rectification. + */ + Vector3 get_on_great_tangent_triangle(Ref next, Vector3 input) const; + double get_tangent_circle_radius_next(); + Vector3 get_tangent_circle_center_next_1(); + Vector3 get_tangent_circle_center_next_2(); + + /** + * returns null if no rectification is required. + * @param input + * @param in_bounds + * @return + */ + Vector3 closest_to_cone(Vector3 input, Vector *in_bounds) const; + Vector3 get_closest_path_point(Ref next, Vector3 input) const; + Vector3 get_control_point() const; + void set_control_point(Vector3 p_control_point); + double get_radius() const; + double get_radius_cosine() const; + void set_radius(double radius); +}; + +#endif // IK_LIMIT_CONE_3D_H diff --git a/modules/many_bone_ik/src/ik_ray_3d.cpp b/modules/many_bone_ik/src/ik_ray_3d.cpp new file mode 100644 index 000000000000..4ec35a56cc11 --- /dev/null +++ b/modules/many_bone_ik/src/ik_ray_3d.cpp @@ -0,0 +1,212 @@ +/**************************************************************************/ +/* ik_ray_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_ray_3d.h" + +IKRay3D::IKRay3D() { +} + +IKRay3D::IKRay3D(Vector3 p_p1, Vector3 p_p2) { + working_vector = p_p1; + point_1 = p_p1; + point_2 = p_p2; +} + +Vector3 IKRay3D::get_heading() { + working_vector = point_2; + return working_vector - point_1; +} + +void IKRay3D::set_heading(const Vector3 &p_new_head) { + point_2 = point_1; + point_2 = p_new_head; +} + +real_t IKRay3D::get_scaled_projection(const Vector3 p_input) { + working_vector = p_input; + working_vector = working_vector - point_1; + Vector3 heading = get_heading(); + real_t headingMag = heading.length(); + real_t workingVectorMag = working_vector.length(); + if (workingVectorMag == 0 || headingMag == 0) { + return 0; + } + return (working_vector.dot(heading) / (headingMag * workingVectorMag)) * (workingVectorMag / headingMag); +} + +void IKRay3D::elongate(real_t amt) { + Vector3 midPoint = (point_1 + point_2) * 0.5f; + Vector3 p1Heading = point_1 - midPoint; + Vector3 p2Heading = point_2 - midPoint; + Vector3 p1Add = p1Heading.normalized() * amt; + Vector3 p2Add = p2Heading.normalized() * amt; + + point_1 = p1Heading + p1Add + midPoint; + point_2 = p2Heading + p2Add + midPoint; +} + +Vector3 IKRay3D::get_intersects_plane(Vector3 ta, Vector3 tb, Vector3 tc) { + Vector3 uvw; + tta = ta; + ttb = tb; + ttc = tc; + tta -= point_1; + ttb -= point_1; + ttc -= point_1; + Vector3 result = plane_intersect_test(tta, ttb, ttc, &uvw); + return result + point_1; +} + +int IKRay3D::intersects_sphere(Vector3 sphereCenter, real_t radius, Vector3 *S1, Vector3 *S2) { + Vector3 tp1 = point_1 - sphereCenter; + Vector3 tp2 = point_2 - sphereCenter; + int result = intersects_sphere(tp1, tp2, radius, S1, S2); + *S1 += sphereCenter; + *S2 += sphereCenter; + return result; +} + +void IKRay3D::set_point_1(Vector3 in) { + point_1 = in; +} + +void IKRay3D::set_point_2(Vector3 in) { + point_2 = in; +} + +Vector3 IKRay3D::get_point_2() { + return point_2; +} + +Vector3 IKRay3D::get_point_1() { + return point_1; +} + +int IKRay3D::intersects_sphere(Vector3 rp1, Vector3 rp2, real_t radius, Vector3 *S1, Vector3 *S2) { + Vector3 direction = rp2 - rp1; + Vector3 e = direction; // e=ray.dir + e.normalize(); // e=g/|g| + Vector3 h = point_1; + h = Vector3(0.0f, 0.0f, 0.0f); + h = h - rp1; // h=r.o-c.M + real_t lf = e.dot(h); // lf=e.h + real_t radpow = radius * radius; + real_t hdh = h.length_squared(); + real_t lfpow = lf * lf; + real_t s = radpow - hdh + lfpow; // s=r^2-h^2+lf^2 + if (s < 0.0f) { + return 0; // no intersection points ? + } + s = Math::sqrt(s); // s=sqrt(r^2-h^2+lf^2) + + int result = 0; + if (lf < s) { + if (lf + s >= 0) { + s = -s; // swap S1 <-> S2} + result = 1; // one intersection point + } + } else { + result = 2; // 2 intersection points + } + + *S1 = e * (lf - s); + *S1 += rp1; // S1=A+e*(lf-s) + *S2 = e * (lf + s); + *S2 += rp1; // S2=A+e*(lf+s) + return result; +} + +Vector3 IKRay3D::plane_intersect_test(Vector3 ta, Vector3 tb, Vector3 tc, Vector3 *uvw) { + u = tb; + v = tc; + n = Vector3(0, 0, 0); + dir = get_heading(); + w0 = Vector3(0, 0, 0); + real_t r, a, b; + u -= ta; + v -= ta; + + n = u.cross(v).normalized(); + + w0 -= ta; + a = -(n.dot(w0)); + b = n.dot(dir); + r = a / b; + I = dir; + I *= r; + barycentric(ta, tb, tc, I, uvw); + return I; +} + +real_t IKRay3D::triangle_area_2d(real_t x1, real_t y1, real_t x2, real_t y2, real_t x3, real_t y3) { + return (x1 - x2) * (y2 - y3) - (x2 - x3) * (y1 - y2); +} + +void IKRay3D::barycentric(Vector3 a, Vector3 b, Vector3 c, Vector3 p, Vector3 *uvw) { + bc = b; + ca = a; + at = a; + bt = b; + ct = c; + pt = p; + + m = Vector3(bc - ct).cross(ca - at).normalized(); + + real_t nu; + real_t nv; + real_t ood; + + real_t x = Math::abs(m.x); + real_t y = Math::abs(m.y); + real_t z = Math::abs(m.z); + + if (x >= y && x >= z) { + nu = triangle_area_2d(pt.y, pt.z, bt.y, bt.z, ct.y, ct.z); + nv = triangle_area_2d(pt.y, pt.z, ct.y, ct.z, at.y, at.z); + ood = 1.0f / m.x; + } else if (y >= x && y >= z) { + nu = triangle_area_2d(pt.x, pt.z, bt.x, bt.z, ct.x, ct.z); + nv = triangle_area_2d(pt.x, pt.z, ct.x, ct.z, at.x, at.z); + ood = 1.0f / -m.y; + } else { + nu = triangle_area_2d(pt.x, pt.y, bt.x, bt.y, ct.x, ct.y); + nv = triangle_area_2d(pt.x, pt.y, ct.x, ct.y, at.x, at.y); + ood = 1.0f / m.z; + } + (*uvw)[0] = nu * ood; + (*uvw)[1] = nv * ood; + (*uvw)[2] = 1.0f - (*uvw)[0] - (*uvw)[1]; +} + +void IKRay3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_heading"), &IKRay3D::get_heading); + ClassDB::bind_method(D_METHOD("get_scaled_projection", "input"), &IKRay3D::get_scaled_projection); + ClassDB::bind_method(D_METHOD("get_intersects_plane", "a", "b", "c"), &IKRay3D::get_intersects_plane); +} diff --git a/modules/many_bone_ik/src/ik_ray_3d.h b/modules/many_bone_ik/src/ik_ray_3d.h new file mode 100644 index 000000000000..4aa57a4058a8 --- /dev/null +++ b/modules/many_bone_ik/src/ik_ray_3d.h @@ -0,0 +1,128 @@ +/**************************************************************************/ +/* ik_ray_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. */ +/**************************************************************************/ + +#ifndef IK_RAY_3D_H +#define IK_RAY_3D_H + +#include "core/io/resource.h" +#include "core/math/vector3.h" + +class IKRay3D : public RefCounted { + GDCLASS(IKRay3D, RefCounted); + + Vector3 tta, ttb, ttc; + Vector3 I, u, v, n, dir, w0; + Vector3 m, at, bt, ct, pt; + Vector3 bc, ca, ac; + + Vector3 point_1; + Vector3 point_2; + Vector3 working_vector; + +protected: + static void _bind_methods(); + +public: + IKRay3D(); + ~IKRay3D() {} + IKRay3D(Vector3 p_point_one, Vector3 p_point_two); + Vector3 get_heading(); + void set_heading(const Vector3 &p_new_head); + + /** + * Returns the scalar projection of the input vector on this + * ray. In other words, if this ray goes from (5, 0) to (10, 0), + * and the input vector is (7.5, 7), this function + * would output 0.5. Because that is amount the ray would need + * to be scaled by so that its tip is where the vector would project onto + * this ray. + *

+ * Due to floating point errors, the intended properties of this function might + * not be entirely consistent with its output under summation. + *

+ * To help spare programmer cognitive cycles debugging in such circumstances, + * the intended properties + * are listed for reference here (despite their being easily inferred). + *

+ * 1. calling get_scaled_projection(someVector) should return the same value as + * calling + * get_scaled_projection(closestPointTo(someVector). + * 2. calling getMultipliedBy(get_scaled_projection(someVector)) should return the + * same + * vector as calling closestPointTo(someVector) + * + * @param p_input a vector to project onto this ray + */ + real_t get_scaled_projection(const Vector3 p_input); + + /** + * adds the specified length to the ray in both directions. + */ + void elongate(real_t p_amount); + + /** + * @param ta the first vertex of a triangle on the plane + * @param tb the second vertex of a triangle on the plane + * @param tc the third vertex of a triangle on the plane + * @return the point where this ray intersects the plane specified by the + * triangle ta,tb,tc. + */ + Vector3 get_intersects_plane(Vector3 p_vertex_a, Vector3 p_vertex_b, Vector3 p_vertex_c); + + /* + * Find where this ray intersects a sphere + * + * @param Vector3 the center of the sphere to test against. + * + * @param radius radius of the sphere + * + * @param S1 reference to variable in which the first intersection will be + * placed + * + * @param S2 reference to variable in which the second intersection will be + * placed + * + * @return number of intersections found; + */ + int intersects_sphere(Vector3 p_sphere_center, real_t p_radius, Vector3 *r_first_intersection, Vector3 *r_second_intersection); + void set_point_1(Vector3 p_point); + void set_point_2(Vector3 p_point); + Vector3 get_point_2(); + Vector3 get_point_1(); + int intersects_sphere(Vector3 p_rp1, Vector3 p_rp2, real_t p_radius, Vector3 *r_first_intersection, Vector3 *r_second_intersection); + real_t triangle_area_2d(real_t p_x1, real_t p_y1, real_t p_x2, real_t p_y2, real_t p_x3, real_t p_y3); + void barycentric(Vector3 p_a, Vector3 p_b, Vector3 p_c, Vector3 p_p, Vector3 *r_uvw); + Vector3 plane_intersect_test(Vector3 p_vertex_a, Vector3 p_vertex_b, Vector3 p_vertex_c, Vector3 *uvw); + operator String() const { + return String(L"(") + point_1.x + L" -> " + point_2.x + L") \n " + L"(" + point_1.y + L" -> " + point_2.y + L") \n " + L"(" + point_1.z + L" -> " + point_2.z + L") \n "; + } +}; + +#endif // IK_RAY_3D_H diff --git a/modules/many_bone_ik/src/many_bone_ik_3d.cpp b/modules/many_bone_ik/src/many_bone_ik_3d.cpp new file mode 100644 index 000000000000..83ef9850b405 --- /dev/null +++ b/modules/many_bone_ik/src/many_bone_ik_3d.cpp @@ -0,0 +1,1458 @@ +/**************************************************************************/ +/* many_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 "many_bone_ik_3d.h" +#include "core/core_string_names.h" +#include "core/error/error_macros.h" +#include "core/io/json.h" +#include "core/object/class_db.h" +#include "core/string/string_name.h" +#include "core/variant/typed_array.h" +#include "ik_bone_3d.h" +#include "ik_kusudama_3d.h" +#include "ik_limit_cone_3d.h" +#include "scene/3d/marker_3d.h" +#include "scene/3d/node_3d.h" +#include "scene/3d/physics_body_3d.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/main/node.h" +#include "scene/main/scene_tree.h" +#include "scene/resources/skeleton_profile.h" +#include "scene/scene_string_names.h" + +#include "skeleton_profile_humanoid_constraint.h" + +#ifdef TOOLS_ENABLED +#include "editor/editor_node.h" +#endif + +void ManyBoneIK3D::set_pin_count(int32_t p_value) { + int32_t old_count = pins.size(); + pin_count = p_value; + pins.resize(p_value); + for (int32_t pin_i = p_value; pin_i-- > old_count;) { + pins.write[pin_i].instantiate(); + } + set_dirty(); +} + +int32_t ManyBoneIK3D::get_pin_count() const { + return pin_count; +} + +void ManyBoneIK3D::set_pin_bone(int32_t p_pin_index, const String &p_bone) { + ERR_FAIL_INDEX(p_pin_index, pins.size()); + Ref effector_template = pins[p_pin_index]; + if (effector_template.is_null()) { + effector_template.instantiate(); + pins.write[p_pin_index] = effector_template; + } + effector_template->set_name(p_bone); + set_dirty(); +} + +void ManyBoneIK3D::set_pin_target_nodepath(int32_t p_pin_index, const NodePath &p_target_node) { + ERR_FAIL_INDEX(p_pin_index, pins.size()); + Ref effector_template = pins[p_pin_index]; + if (effector_template.is_null()) { + effector_template.instantiate(); + pins.write[p_pin_index] = effector_template; + } + effector_template->set_target_node(p_target_node); + set_dirty(); +} + +NodePath ManyBoneIK3D::get_pin_target_nodepath(int32_t p_pin_index) { + ERR_FAIL_INDEX_V(p_pin_index, pins.size(), NodePath()); + const Ref effector_template = pins[p_pin_index]; + return effector_template->get_target_node(); +} + +Vector> ManyBoneIK3D::get_bone_effectors() const { + return pins; +} + +void ManyBoneIK3D::_remove_pin(int32_t p_index) { + ERR_FAIL_INDEX(p_index, pins.size()); + pins.remove_at(p_index); + pin_count--; + pins.resize(pin_count); + set_dirty(); +} + +void ManyBoneIK3D::update_ik_bones_transform() { + for (int32_t bone_i = bone_list.size(); bone_i-- > 0;) { + Ref bone = bone_list[bone_i]; + if (bone.is_null()) { + continue; + } + bone->set_initial_pose(get_skeleton()); + if (bone->is_pinned()) { + bone->get_pin()->update_target_global_transform(get_skeleton(), this); + } + } +} + +void ManyBoneIK3D::update_skeleton_bones_transform() { + for (int32_t bone_i = bone_list.size(); bone_i-- > 0;) { + Ref bone = bone_list[bone_i]; + if (bone.is_null()) { + continue; + } + if (bone->get_bone_id() == -1) { + continue; + } + bone->set_skeleton_bone_pose(get_skeleton()); + } +} + +void ManyBoneIK3D::_get_property_list(List *p_list) const { + const Vector> ik_bones = get_bone_list(); + RBSet existing_constraints; + for (int32_t constraint_i = 0; constraint_i < ik_bones.size(); constraint_i++) { + const String name = ik_bones[constraint_i]->get_name(); + existing_constraints.insert(name); + } + uint32_t constraint_usage = PROPERTY_USAGE_DEFAULT; + p_list->push_back( + PropertyInfo(Variant::INT, "constraint_count", + PROPERTY_HINT_RANGE, "0,256,or_greater", constraint_usage | PROPERTY_USAGE_ARRAY | PROPERTY_USAGE_READ_ONLY, + "Kusudama Constraints,constraints/")); + for (int constraint_i = 0; constraint_i < get_constraint_count(); constraint_i++) { + PropertyInfo bone_name; + bone_name.type = Variant::STRING_NAME; + bone_name.usage = constraint_usage; + bone_name.name = "constraints/" + itos(constraint_i) + "/bone_name"; + if (get_skeleton()) { + String names; + for (int bone_i = 0; bone_i < get_constraint_count(); bone_i++) { + String name = get_constraint_name(bone_i); + if (existing_constraints.has(name)) { + continue; + } + name += ","; + names += name; + existing_constraints.insert(name); + } + bone_name.hint = PROPERTY_HINT_ENUM_SUGGESTION; + bone_name.hint_string = names; + } else { + bone_name.hint = PROPERTY_HINT_NONE; + bone_name.hint_string = ""; + } + p_list->push_back(bone_name); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/resistance", PROPERTY_HINT_RANGE, "0,1,0.01,exp", constraint_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/twist_from", PROPERTY_HINT_RANGE, "-359.9,359.9,0.1,radians,exp", constraint_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/twist_range", PROPERTY_HINT_RANGE, "-359.9,359.9,0.1,radians,exp", constraint_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/twist_current", PROPERTY_HINT_RANGE, "-359.9,359.9,0.1,radians,exp", constraint_usage)); + p_list->push_back( + PropertyInfo(Variant::INT, "constraints/" + itos(constraint_i) + "/kusudama_limit_cone_count", PROPERTY_HINT_RANGE, "0,10,1", constraint_usage | PROPERTY_USAGE_ARRAY | PROPERTY_USAGE_READ_ONLY, + "Limit Cones,constraints/" + itos(constraint_i) + "/kusudama_limit_cone/")); + for (int cone_i = 0; cone_i < get_kusudama_limit_cone_count(constraint_i); cone_i++) { + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/kusudama_limit_cone/" + itos(cone_i) + "/altitude", PROPERTY_HINT_RANGE, "-90,90.0,0.1,radians,exp", constraint_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/kusudama_limit_cone/" + itos(cone_i) + "/azimuth", PROPERTY_HINT_RANGE, "0,359.9,0.1,radians,exp", constraint_usage)); + + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/kusudama_limit_cone/" + itos(cone_i) + "/radius", PROPERTY_HINT_RANGE, "0,180,0.1,radians,exp", constraint_usage)); + } + p_list->push_back( + PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/kusudama_twist", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); + p_list->push_back( + PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/kusudama_orientation", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); + p_list->push_back( + PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/bone_direction", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); + } + RBSet existing_pins; + for (int32_t pin_i = 0; pin_i < get_pin_count(); pin_i++) { + const String name = get_pin_bone_name(pin_i); + existing_pins.insert(name); + } + const uint32_t pin_usage = PROPERTY_USAGE_DEFAULT; + p_list->push_back( + PropertyInfo(Variant::INT, "pin_count", + PROPERTY_HINT_RANGE, "0,65536,or_greater", pin_usage | PROPERTY_USAGE_ARRAY | PROPERTY_USAGE_READ_ONLY, + "Pins,pins/")); + for (int pin_i = 0; pin_i < get_pin_count(); pin_i++) { + PropertyInfo effector_name; + effector_name.type = Variant::STRING_NAME; + effector_name.name = "pins/" + itos(pin_i) + "/bone_name"; + effector_name.usage = pin_usage | PROPERTY_USAGE_READ_ONLY; + if (get_skeleton()) { + String names; + for (int bone_i = 0; bone_i < get_skeleton()->get_bone_count(); bone_i++) { + String name = get_skeleton()->get_bone_name(bone_i); + StringName string_name = StringName(name); + if (existing_pins.has(string_name)) { + continue; + } + } + effector_name.hint = PROPERTY_HINT_ENUM_SUGGESTION; + effector_name.hint_string = names; + } else { + effector_name.hint = PROPERTY_HINT_NONE; + effector_name.hint_string = ""; + } + p_list->push_back(effector_name); + p_list->push_back( + PropertyInfo(Variant::NODE_PATH, "pins/" + itos(pin_i) + "/target_node", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D", pin_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "pins/" + itos(pin_i) + "/passthrough_factor", PROPERTY_HINT_RANGE, "0,1,0.1,or_greater", pin_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "pins/" + itos(pin_i) + "/weight", PROPERTY_HINT_RANGE, "0,1,0.1,or_greater", pin_usage)); + p_list->push_back( + PropertyInfo(Variant::VECTOR3, "pins/" + itos(pin_i) + "/direction_priorities", PROPERTY_HINT_RANGE, "0,1,0.1,or_greater", pin_usage)); + } +} + +bool ManyBoneIK3D::_get(const StringName &p_name, Variant &r_ret) const { + String name = p_name; + if (name == "constraint_count") { + r_ret = get_constraint_count(); + return true; + } else if (name == "pin_count") { + r_ret = get_pin_count(); + return true; + } else if (name == "bone_count") { + r_ret = get_bone_count(); + return true; + } else if (name.begins_with("pins/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + ERR_FAIL_INDEX_V(index, pins.size(), false); + Ref effector_template = pins[index]; + ERR_FAIL_NULL_V(effector_template, false); + if (what == "bone_name") { + r_ret = effector_template->get_name(); + return true; + } else if (what == "target_node") { + r_ret = effector_template->get_target_node(); + return true; + } else if (what == "passthrough_factor") { + r_ret = get_pin_passthrough_factor(index); + return true; + } else if (what == "weight") { + r_ret = get_pin_weight(index); + return true; + } else if (what == "direction_priorities") { + r_ret = get_pin_direction_priorities(index); + return true; + } + } else if (name.begins_with("constraints/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + ERR_FAIL_INDEX_V(index, constraint_count, false); + String begins = "constraints/" + itos(index) + "/kusudama_limit_cone"; + if (what == "bone_name") { + ERR_FAIL_INDEX_V(index, constraint_names.size(), false); + r_ret = constraint_names[index]; + return true; + } else if (what == "resistance") { + r_ret = get_kusudama_resistance(index); + return true; + } else if (what == "twist_from") { + r_ret = get_kusudama_twist(index).x; + return true; + } else if (what == "twist_range") { + r_ret = get_kusudama_twist(index).y; + return true; + } else if (what == "twist_current") { + r_ret = get_kusudama_twist_current(index); + return true; + } else if (what == "kusudama_limit_cone_count") { + r_ret = get_kusudama_limit_cone_count(index); + return true; + } else if (name.begins_with(begins)) { + int32_t cone_index = name.get_slicec('/', 3).to_int(); + String cone_what = name.get_slicec('/', 4); + if (cone_what == "altitude") { + Vector3 center = get_kusudama_limit_cone_center(index, cone_index); + r_ret = convert_coordinate_to_attitude_azimuth(center).x; + return true; + } else if (cone_what == "azimuth") { + Vector3 center = get_kusudama_limit_cone_center(index, cone_index); + r_ret = convert_coordinate_to_attitude_azimuth(center).y; + return true; + } else if (cone_what == "radius") { + r_ret = get_kusudama_limit_cone_radius(index, cone_index); + return true; + } + } else if (what == "bone_direction") { + r_ret = get_bone_direction_transform(index); + return true; + } else if (what == "kusudama_orientation") { + r_ret = get_constraint_orientation_transform(index); + return true; + } else if (what == "kusudama_twist") { + r_ret = get_constraint_twist_transform(index); + return true; + } + } + return false; +} + +bool ManyBoneIK3D::_set(const StringName &p_name, const Variant &p_value) { + String name = p_name; + if (name == "constraint_count") { + set_constraint_count(p_value); + return true; + } else if (name == "pin_count") { + set_pin_count(p_value); + return true; + } else if (name.begins_with("pins/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + if (index >= pins.size()) { + set_pin_count(constraint_count); + } + if (what == "bone_name") { + set_pin_bone(index, p_value); + return true; + } else if (what == "target_node") { + set_pin_target_nodepath(index, p_value); + String existing_bone = get_pin_bone_name(index); + if (existing_bone.is_empty()) { + return false; + } + return true; + } else if (what == "passthrough_factor") { + set_pin_passthrough_factor(index, p_value); + return true; + } else if (what == "weight") { + set_pin_weight(index, p_value); + return true; + } else if (what == "direction_priorities") { + set_pin_direction_priorities(index, p_value); + return true; + } + } else if (name.begins_with("constraints/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + String begins = "constraints/" + itos(index) + "/kusudama_limit_cone/"; + if (index >= constraint_names.size()) { + set_constraint_count(constraint_count); + } + if (what == "bone_name") { + set_constraint_name(index, p_value); + return true; + } else if (what == "resistance") { + set_kusudama_resistance(index, p_value); + return true; + } else if (what == "twist_from") { + float new_twist_from = p_value; + Vector2 twist = get_kusudama_twist(index); + float new_twist_current = get_kusudama_twist_current(index); + if (new_twist_current < twist.x || new_twist_current > twist.x + twist.y) { + twist.x = MIN(new_twist_current, twist.x); + twist.y = MAX(new_twist_current, twist.y); + } + set_kusudama_twist(index, Vector2(new_twist_from, twist.y)); + set_kusudama_twist_current(index, new_twist_current); + return true; + } else if (what == "twist_range") { + float new_twist_range = p_value; + Vector2 twist = get_kusudama_twist(index); + float new_twist_current = get_kusudama_twist_current(index); + if (new_twist_current < twist.x || new_twist_current > twist.x + twist.y) { + twist.x = MIN(new_twist_current, twist.x); + twist.y = MAX(new_twist_current, twist.y); + } + set_kusudama_twist(index, Vector2(twist.x, new_twist_range)); + set_kusudama_twist_current(index, new_twist_current); + return true; + } else if (what == "twist_current") { + float new_twist_current = p_value; + Vector2 twist = get_kusudama_twist(index); + set_kusudama_twist(index, Vector2(new_twist_current, twist.y)); + set_kusudama_twist_current(index, new_twist_current); + return true; + } else if (what == "kusudama_limit_cone_count") { + set_kusudama_limit_cone_count(index, p_value); + return true; + } else if (name.begins_with(begins)) { + int cone_index = name.get_slicec('/', 3).to_int(); + String cone_what = name.get_slicec('/', 4); + if (cone_what == "altitude") { + float attitude = p_value; + Vector3 center = get_kusudama_limit_cone_center(index, cone_index); + Vector2 attitude_azimuth = convert_coordinate_to_attitude_azimuth(center); + Vector3 new_center = convert_attitude_azimuth_to_coordinate(attitude, attitude_azimuth.y); + set_kusudama_limit_cone_center(index, cone_index, new_center); + return true; + } else if (cone_what == "azimuth") { + float azimuth = p_value; + Vector3 center = get_kusudama_limit_cone_center(index, cone_index); + Vector2 attitude_azimuth = convert_coordinate_to_attitude_azimuth(center); + Vector3 new_center = convert_attitude_azimuth_to_coordinate(attitude_azimuth.x, azimuth); + set_kusudama_limit_cone_center(index, cone_index, new_center); + return true; + } else if (cone_what == "radius") { + set_kusudama_limit_cone_radius(index, cone_index, p_value); + return true; + } + } else if (what == "bone_direction") { + set_bone_direction_transform(index, p_value); + return true; + } else if (what == "kusudama_orientation") { + set_constraint_orientation_transform(index, p_value); + return true; + } else if (what == "kusudama_twist") { + set_constraint_twist_transform(index, p_value); + return true; + } + } + + return false; +} + +void ManyBoneIK3D::_bind_methods() { + ClassDB::bind_static_method("ManyBoneIK3D", D_METHOD("convert_attitude_azimuth_to_coordinate", "atitude", "azimuth"), &ManyBoneIK3D::convert_attitude_azimuth_to_coordinate); + ClassDB::bind_static_method("ManyBoneIK3D", D_METHOD("convert_coordinate_to_attitude_azimuth", "center"), &ManyBoneIK3D::convert_coordinate_to_attitude_azimuth); + ClassDB::bind_method(D_METHOD("set_kusudama_resistance", "index", "resistance"), &ManyBoneIK3D::set_kusudama_resistance); + ClassDB::bind_method(D_METHOD("get_kusudama_resistance", "index"), &ManyBoneIK3D::get_kusudama_resistance); + ClassDB::bind_method(D_METHOD("get_constraint_twist_transform", "index"), &ManyBoneIK3D::get_constraint_twist_transform); + ClassDB::bind_method(D_METHOD("set_constraint_twist_transform", "index", "transform"), &ManyBoneIK3D::set_constraint_twist_transform); + ClassDB::bind_method(D_METHOD("get_constraint_orientation_transform", "index"), &ManyBoneIK3D::get_constraint_orientation_transform); + ClassDB::bind_method(D_METHOD("set_constraint_orientation_transform", "index", "transform"), &ManyBoneIK3D::set_constraint_orientation_transform); + ClassDB::bind_method(D_METHOD("get_bone_direction_transform", "index"), &ManyBoneIK3D::get_bone_direction_transform); + ClassDB::bind_method(D_METHOD("set_bone_direction_transform", "index", "transform"), &ManyBoneIK3D::set_bone_direction_transform); + ClassDB::bind_method(D_METHOD("get_pin_enabled", "index"), &ManyBoneIK3D::get_pin_enabled); + ClassDB::bind_method(D_METHOD("remove_constraint", "index"), &ManyBoneIK3D::remove_constraint); + ClassDB::bind_method(D_METHOD("set_skeleton_node_path", "path"), &ManyBoneIK3D::set_skeleton_node_path); + ClassDB::bind_method(D_METHOD("get_skeleton_node_path"), &ManyBoneIK3D::get_skeleton_node_path); + ClassDB::bind_method(D_METHOD("register_skeleton"), &ManyBoneIK3D::register_skeleton); + ClassDB::bind_method(D_METHOD("reset_constraints"), &ManyBoneIK3D::register_skeleton); + ClassDB::bind_method(D_METHOD("set_pin_weight", "index", "weight"), &ManyBoneIK3D::set_pin_weight); + ClassDB::bind_method(D_METHOD("get_pin_weight", "index"), &ManyBoneIK3D::get_pin_weight); + ClassDB::bind_method(D_METHOD("set_dirty"), &ManyBoneIK3D::set_dirty); + ClassDB::bind_method(D_METHOD("set_kusudama_limit_cone_radius", "index", "cone_index", "radius"), &ManyBoneIK3D::set_kusudama_limit_cone_radius); + ClassDB::bind_method(D_METHOD("get_kusudama_limit_cone_radius", "index", "cone_index"), &ManyBoneIK3D::get_kusudama_limit_cone_radius); + ClassDB::bind_method(D_METHOD("set_kusudama_limit_cone_center", "index", "cone_index", "center"), &ManyBoneIK3D::set_kusudama_limit_cone_center); + ClassDB::bind_method(D_METHOD("get_kusudama_limit_cone_center", "index", "cone_index"), &ManyBoneIK3D::get_kusudama_limit_cone_center); + ClassDB::bind_method(D_METHOD("set_kusudama_limit_cone_count", "index", "count"), &ManyBoneIK3D::set_kusudama_limit_cone_count); + ClassDB::bind_method(D_METHOD("get_kusudama_limit_cone_count", "index"), &ManyBoneIK3D::get_kusudama_limit_cone_count); + ClassDB::bind_method(D_METHOD("set_kusudama_twist", "index", "limit"), &ManyBoneIK3D::set_kusudama_twist); + ClassDB::bind_method(D_METHOD("get_kusudama_twist", "index"), &ManyBoneIK3D::get_kusudama_twist); + ClassDB::bind_method(D_METHOD("set_pin_passthrough_factor", "index", "falloff"), &ManyBoneIK3D::set_pin_passthrough_factor); + ClassDB::bind_method(D_METHOD("get_pin_passthrough_factor", "index"), &ManyBoneIK3D::get_pin_passthrough_factor); + ClassDB::bind_method(D_METHOD("get_constraint_name", "index"), &ManyBoneIK3D::get_constraint_name); + ClassDB::bind_method(D_METHOD("get_iterations_per_frame"), &ManyBoneIK3D::get_iterations_per_frame); + ClassDB::bind_method(D_METHOD("set_iterations_per_frame", "count"), &ManyBoneIK3D::set_iterations_per_frame); + ClassDB::bind_method(D_METHOD("find_constraint", "name"), &ManyBoneIK3D::find_constraint); + ClassDB::bind_method(D_METHOD("get_constraint_count"), &ManyBoneIK3D::get_constraint_count); + ClassDB::bind_method(D_METHOD("get_pin_count"), &ManyBoneIK3D::get_pin_count); + ClassDB::bind_method(D_METHOD("get_pin_bone_name", "index"), &ManyBoneIK3D::get_pin_bone_name); + ClassDB::bind_method(D_METHOD("get_pin_direction_priorities", "index"), &ManyBoneIK3D::get_pin_direction_priorities); + ClassDB::bind_method(D_METHOD("set_pin_direction_priorities", "index", "priority"), &ManyBoneIK3D::set_pin_direction_priorities); + ClassDB::bind_method(D_METHOD("queue_print_skeleton"), &ManyBoneIK3D::queue_print_skeleton); + ClassDB::bind_method(D_METHOD("get_default_damp"), &ManyBoneIK3D::get_default_damp); + ClassDB::bind_method(D_METHOD("set_default_damp", "damp"), &ManyBoneIK3D::set_default_damp); + ClassDB::bind_method(D_METHOD("get_pin_nodepath", "index"), &ManyBoneIK3D::get_pin_nodepath); + ClassDB::bind_method(D_METHOD("set_pin_nodepath", "index", "nodepath"), &ManyBoneIK3D::set_pin_nodepath); + ClassDB::bind_method(D_METHOD("get_bone_count"), &ManyBoneIK3D::get_bone_count); + ClassDB::bind_method(D_METHOD("set_constraint_mode", "enabled"), &ManyBoneIK3D::set_constraint_mode); + ClassDB::bind_method(D_METHOD("get_constraint_mode"), &ManyBoneIK3D::get_constraint_mode); + ClassDB::bind_method(D_METHOD("get_kusudama_twist_current"), &ManyBoneIK3D::get_kusudama_twist_current); + ClassDB::bind_method(D_METHOD("set_kusudama_twist_current", "twist_current"), &ManyBoneIK3D::get_kusudama_twist_current); + ClassDB::bind_method(D_METHOD("set_ui_selected_bone", "bone"), &ManyBoneIK3D::set_ui_selected_bone); + ClassDB::bind_method(D_METHOD("get_ui_selected_bone"), &ManyBoneIK3D::get_ui_selected_bone); + ClassDB::bind_method(D_METHOD("set_twist_constraint_defaults", "defaults"), &ManyBoneIK3D::set_twist_constraint_defaults); + ClassDB::bind_method(D_METHOD("get_twist_constraint_defaults"), &ManyBoneIK3D::get_twist_constraint_defaults); + ClassDB::bind_method(D_METHOD("set_orientation_constraint_defaults", "defaults"), &ManyBoneIK3D::set_orientation_constraint_defaults); + ClassDB::bind_method(D_METHOD("get_orientation_constraint_defaults"), &ManyBoneIK3D::get_orientation_constraint_defaults); + ClassDB::bind_method(D_METHOD("set_bone_direction_constraint_defaults", "defaults"), &ManyBoneIK3D::set_bone_direction_constraint_defaults); + ClassDB::bind_method(D_METHOD("get_bone_direction_constraint_defaults"), &ManyBoneIK3D::get_bone_direction_constraint_defaults); + ClassDB::bind_method(D_METHOD("set_stabilization_passes", "passes"), &ManyBoneIK3D::set_stabilization_passes); + ClassDB::bind_method(D_METHOD("get_stabilization_passes"), &ManyBoneIK3D::get_stabilization_passes); + + ClassDB::bind_method(D_METHOD("setup_humanoid_bones", "enable"), &ManyBoneIK3D::setup_humanoid_bones); + + ClassDB::bind_method(D_METHOD("set_setup_humanoid_bones", "set_targets"), &ManyBoneIK3D::set_setup_humanoid_bones); + ClassDB::bind_method(D_METHOD("get_setup_humanoid_bones"), &ManyBoneIK3D::get_setup_humanoid_bones); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "initialize_humanoid_bones"), "set_setup_humanoid_bones", "get_setup_humanoid_bones"); + + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "skeleton_node_path"), "set_skeleton_node_path", "get_skeleton_node_path"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "iterations_per_frame", PROPERTY_HINT_RANGE, "1,150,1,or_greater"), "set_iterations_per_frame", "get_iterations_per_frame"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "default_damp", PROPERTY_HINT_RANGE, "0.01,180.0,0.1,radians,exp", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_UPDATE_ALL_IF_MODIFIED), "set_default_damp", "get_default_damp"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "constraint_mode"), "set_constraint_mode", "get_constraint_mode"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "ui_selected_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_ui_selected_bone", "get_ui_selected_bone"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "stabilization_passes"), "set_stabilization_passes", "get_stabilization_passes"); + ADD_PROPERTY(PropertyInfo(Variant::DICTIONARY, "twist_constraint_defaults", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_twist_constraint_defaults", "get_twist_constraint_defaults"); + ADD_PROPERTY(PropertyInfo(Variant::DICTIONARY, "orientation_constraint_defaults", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_orientation_constraint_defaults", "get_orientation_constraint_defaults"); + ADD_PROPERTY(PropertyInfo(Variant::DICTIONARY, "bone_direction_constraint_defaults", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_bone_direction_constraint_defaults", "get_bone_direction_constraint_defaults"); +} + +ManyBoneIK3D::ManyBoneIK3D() { +} + +ManyBoneIK3D::~ManyBoneIK3D() { +} + +void ManyBoneIK3D::queue_print_skeleton() { + queue_debug_skeleton = true; +} + +float ManyBoneIK3D::get_pin_passthrough_factor(int32_t p_effector_index) const { + ERR_FAIL_INDEX_V(p_effector_index, pins.size(), 0.0f); + const Ref effector_template = pins[p_effector_index]; + return effector_template->get_passthrough_factor(); +} + +void ManyBoneIK3D::set_pin_passthrough_factor(int32_t p_effector_index, const float p_passthrough_factor) { + ERR_FAIL_INDEX(p_effector_index, pins.size()); + Ref effector_template = pins[p_effector_index]; + ERR_FAIL_NULL(effector_template); + effector_template->set_passthrough_factor(p_passthrough_factor); + set_dirty(); +} + +void ManyBoneIK3D::set_constraint_count(int32_t p_count) { + int32_t old_count = constraint_names.size(); + constraint_count = p_count; + constraint_names.resize(p_count); + kusudama_twist.resize(p_count); + kusudama_limit_cone_count.resize(p_count); + kusudama_limit_cones.resize(p_count); + bone_resistance.resize(p_count); + for (int32_t constraint_i = p_count; constraint_i-- > old_count;) { + constraint_names.write[constraint_i] = String(); + kusudama_limit_cone_count.write[constraint_i] = 0; + kusudama_limit_cones.write[constraint_i].resize(1); + kusudama_limit_cones.write[constraint_i].write[0] = Vector4(0, 1, 0, Math_PI); + kusudama_twist.write[constraint_i] = Vector2(0, Math_TAU - CMP_EPSILON); + bone_resistance.write[constraint_i] = 0.0f; + } + set_dirty(); +} + +int32_t ManyBoneIK3D::get_constraint_count() const { + return constraint_count; +} + +inline StringName ManyBoneIK3D::get_constraint_name(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, constraint_names.size(), StringName()); + return constraint_names[p_index]; +} + +Vector2 ManyBoneIK3D::get_kusudama_twist(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, kusudama_twist.size(), Vector2()); + return kusudama_twist[p_index]; +} + +void ManyBoneIK3D::set_kusudama_twist(int32_t p_index, Vector2 p_to) { + ERR_FAIL_INDEX(p_index, constraint_count); + kusudama_twist.write[p_index] = p_to; + set_dirty(); +} + +void ManyBoneIK3D::set_kusudama_twist_from_range(int32_t p_index, float from, float range) { + ERR_FAIL_INDEX(p_index, constraint_count); + + Vector2 p_to = Vector2(from, range); + + kusudama_twist.write[p_index] = p_to; + set_dirty(); +} + +int32_t ManyBoneIK3D::find_effector_id(StringName p_bone_name) { + for (int32_t constraint_i = 0; constraint_i < constraint_count; constraint_i++) { + if (constraint_names[constraint_i] == p_bone_name) { + return constraint_i; + } + } + return -1; +} + +void ManyBoneIK3D::set_kusudama_limit_cone(int32_t p_constraint_index, int32_t p_index, + Vector3 p_center, float p_radius) { + ERR_FAIL_INDEX(p_constraint_index, kusudama_limit_cones.size()); + Vector cones = kusudama_limit_cones.write[p_constraint_index]; + if (Math::is_zero_approx(p_center.length_squared())) { + p_center = Vector3(0.0f, 1.0f, 0.0f); + } + Vector3 center = p_center; + Vector4 cone; + cone.x = center.x; + cone.y = center.y; + cone.z = center.z; + cone.w = p_radius; + cones.write[p_index] = cone; + kusudama_limit_cones.write[p_constraint_index] = cones; + set_dirty(); +} + +Vector3 ManyBoneIK3D::get_kusudama_limit_cone_center(int32_t p_constraint_index, int32_t p_index) const { + if (unlikely((p_constraint_index) < 0 || (p_constraint_index) >= (kusudama_limit_cones.size()))) { + ERR_PRINT_ONCE("Can't get limit cone center."); + return Vector3(0.0, 1.0, 0.0); + } + if (unlikely((p_index) < 0 || (p_index) >= (kusudama_limit_cones[p_constraint_index].size()))) { + ERR_PRINT_ONCE("Can't get limit cone center."); + return Vector3(0.0, 1.0, 0.0); + } + const Vector4 &cone = kusudama_limit_cones[p_constraint_index][p_index]; + Vector3 ret; + ret.x = cone.x; + ret.y = cone.y; + ret.z = cone.z; + return ret; +} + +float ManyBoneIK3D::get_kusudama_limit_cone_radius(int32_t p_constraint_index, int32_t p_index) const { + ERR_FAIL_INDEX_V(p_constraint_index, kusudama_limit_cones.size(), Math_TAU); + ERR_FAIL_INDEX_V(p_index, kusudama_limit_cones[p_constraint_index].size(), Math_TAU); + return kusudama_limit_cones[p_constraint_index][p_index].w; +} + +int32_t ManyBoneIK3D::get_kusudama_limit_cone_count(int32_t p_constraint_index) const { + ERR_FAIL_INDEX_V(p_constraint_index, kusudama_limit_cone_count.size(), 0); + return kusudama_limit_cone_count[p_constraint_index]; +} + +void ManyBoneIK3D::set_kusudama_limit_cone_count(int32_t p_constraint_index, int32_t p_count) { + ERR_FAIL_INDEX(p_constraint_index, kusudama_limit_cone_count.size()); + ERR_FAIL_INDEX(p_constraint_index, kusudama_limit_cones.size()); + int32_t old_cone_count = kusudama_limit_cones[p_constraint_index].size(); + kusudama_limit_cone_count.write[p_constraint_index] = p_count; + Vector &cones = kusudama_limit_cones.write[p_constraint_index]; + cones.resize(p_count); + String bone_name = get_constraint_name(p_constraint_index); + Transform3D bone_transform = get_bone_direction_transform(p_constraint_index); + Vector3 forward_axis = -bone_transform.basis.get_column(Vector3::AXIS_Y).normalized(); + for (int32_t cone_i = p_count; cone_i-- > old_cone_count;) { + Vector4 &cone = cones.write[cone_i]; + cone.x = forward_axis.x; + cone.y = forward_axis.y; + cone.z = forward_axis.z; + cone.w = Math::deg_to_rad(0.0f); + } + set_dirty(); + notify_property_list_changed(); +} + +real_t ManyBoneIK3D::get_default_damp() const { + return default_damp; +} + +void ManyBoneIK3D::set_default_damp(float p_default_damp) { + default_damp = p_default_damp; + set_dirty(); +} + +StringName ManyBoneIK3D::get_pin_bone_name(int32_t p_effector_index) const { + ERR_FAIL_INDEX_V(p_effector_index, pins.size(), ""); + Ref effector_template = pins[p_effector_index]; + return effector_template->get_name(); +} + +void ManyBoneIK3D::set_kusudama_limit_cone_radius(int32_t p_effector_index, int32_t p_index, float p_radius) { + ERR_FAIL_INDEX(p_effector_index, kusudama_limit_cone_count.size()); + ERR_FAIL_INDEX(p_effector_index, kusudama_limit_cones.size()); + ERR_FAIL_INDEX(p_index, kusudama_limit_cone_count[p_effector_index]); + ERR_FAIL_INDEX(p_index, kusudama_limit_cones[p_effector_index].size()); + Vector4 &cone = kusudama_limit_cones.write[p_effector_index].write[p_index]; + cone.w = p_radius; + set_dirty(); +} + +void ManyBoneIK3D::set_kusudama_limit_cone_center(int32_t p_effector_index, int32_t p_index, Vector3 p_center) { + ERR_FAIL_INDEX(p_effector_index, kusudama_limit_cones.size()); + ERR_FAIL_INDEX(p_index, kusudama_limit_cones[p_effector_index].size()); + Vector4 &cone = kusudama_limit_cones.write[p_effector_index].write[p_index]; + if (Math::is_zero_approx(p_center.length_squared())) { + cone.x = 0; + cone.y = 1; + cone.z = 0; + } else { + cone.x = p_center.x; + cone.y = p_center.y; + cone.z = p_center.z; + } + set_dirty(); +} + +void ManyBoneIK3D::set_constraint_name(int32_t p_index, String p_name) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + constraint_names.write[p_index] = p_name; + set_dirty(); +} + +Vector> ManyBoneIK3D::get_segmented_skeletons() { + return segmented_skeletons; +} +float ManyBoneIK3D::get_iterations_per_frame() const { + return iterations_per_frame; +} + +void ManyBoneIK3D::set_iterations_per_frame(const float &p_iterations_per_frame) { + iterations_per_frame = p_iterations_per_frame; +} + +void ManyBoneIK3D::set_pin_bone_name(int32_t p_effector_index, StringName p_name) const { + ERR_FAIL_INDEX(p_effector_index, pins.size()); + Ref effector_template = pins[p_effector_index]; + effector_template->set_name(p_name); +} + +void ManyBoneIK3D::set_pin_nodepath(int32_t p_effector_index, NodePath p_node_path) { + ERR_FAIL_INDEX(p_effector_index, pins.size()); + Node *node = get_node_or_null(p_node_path); + if (!node) { + return; + } + Ref effector_template = pins[p_effector_index]; + effector_template->set_target_node(p_node_path); +} + +NodePath ManyBoneIK3D::get_pin_nodepath(int32_t p_effector_index) const { + ERR_FAIL_INDEX_V(p_effector_index, pins.size(), NodePath()); + Ref effector_template = pins[p_effector_index]; + return effector_template->get_target_node(); +} + +void ManyBoneIK3D::execute(real_t delta) { + if (!get_skeleton()) { + return; + } + if (get_pin_count() == 0) { + return; + } + if (!segmented_skeletons.size()) { + set_dirty(); + } + if (is_dirty) { + skeleton_changed(get_skeleton()); + is_dirty = false; + for (int32_t constraint_i = 0; constraint_i < get_constraint_count(); constraint_i++) { + String constraint_name = get_constraint_name(constraint_i); + twist_constraint_defaults[constraint_name] = get_constraint_twist_transform(constraint_i); + orientation_constraint_defaults[constraint_name] = get_constraint_orientation_transform(constraint_i); + bone_direction_constraint_defaults[constraint_name] = get_bone_direction_transform(constraint_i); + } + } + if (bone_list.size()) { + Ref root_ik_bone = bone_list.write[0]->get_ik_transform(); + if (root_ik_bone.is_null()) { + return; + } + Skeleton3D *skeleton = get_skeleton(); + godot_skeleton_transform.instantiate(); + godot_skeleton_transform->set_transform(skeleton->get_transform()); + godot_skeleton_transform_inverse = skeleton->get_transform().affine_inverse(); + } + bool has_pins = false; + for (Ref pin : pins) { + if (pin.is_valid() && !pin->get_name().is_empty()) { + has_pins = true; + break; + } + } + if (!has_pins) { + return; + } + update_ik_bones_transform(); + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + segmented_skeleton->update_returnfulness_damp(get_iterations_per_frame()); + } + for (int32_t i = 0; i < get_iterations_per_frame(); i++) { + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + segmented_skeleton->segment_solver(bone_damp, get_default_damp(), get_constraint_mode(), i, get_iterations_per_frame()); + } + } + update_skeleton_bones_transform(); +} + +void ManyBoneIK3D::skeleton_changed(Skeleton3D *p_skeleton) { + if (!p_skeleton) { + return; + } + Vector roots = p_skeleton->get_parentless_bones(); + if (roots.is_empty()) { + return; + } + bone_list.clear(); + segmented_skeletons.clear(); + for (BoneId root_bone_index : roots) { + StringName parentless_bone = p_skeleton->get_bone_name(root_bone_index); + Ref segmented_skeleton = Ref(memnew(IKBoneSegment3D(p_skeleton, parentless_bone, pins, this, nullptr, root_bone_index, -1, stabilize_passes))); + ik_origin.instantiate(); + segmented_skeleton->get_root()->get_ik_transform()->set_parent(ik_origin); + segmented_skeleton->generate_default_segments(pins, root_bone_index, -1, this); + Vector> new_bone_list; + segmented_skeleton->create_bone_list(new_bone_list, true, queue_debug_skeleton); + bone_list.append_array(new_bone_list); + Vector> weight_array; + segmented_skeleton->update_pinned_list(weight_array); + segmented_skeleton->recursive_create_headings_arrays_for(segmented_skeleton); + segmented_skeletons.push_back(segmented_skeleton); + } + update_ik_bones_transform(); + for (Ref &ik_bone_3d : bone_list) { + ik_bone_3d->update_default_bone_direction_transform(p_skeleton); + } + for (int constraint_i = 0; constraint_i < constraint_count; ++constraint_i) { + String bone = constraint_names[constraint_i]; + BoneId bone_id = p_skeleton->find_bone(bone); + for (Ref &ik_bone_3d : bone_list) { + if (ik_bone_3d->get_bone_id() != bone_id) { + continue; + } + Ref constraint; + constraint.instantiate(); + constraint->enable_orientational_limits(); + + int32_t cone_count = kusudama_limit_cone_count[constraint_i]; + const Vector &cones = kusudama_limit_cones[constraint_i]; + for (int32_t cone_i = 0; cone_i < cone_count; ++cone_i) { + const Vector4 &cone = cones[cone_i]; + constraint->add_limit_cone(Vector3(cone.x, cone.y, cone.z), cone.w); + } + + const Vector2 axial_limit = get_kusudama_twist(constraint_i); + constraint->enable_axial_limits(); + constraint->set_axial_limits(axial_limit.x, axial_limit.y); + constraint->set_resistance(get_kusudama_resistance(constraint_i)); + ik_bone_3d->add_constraint(constraint); + constraint->_update_constraint(); + break; + } + } + if (!twist_constraint_defaults.size() && !orientation_constraint_defaults.size() && !bone_direction_constraint_defaults.size()) { + for (Ref &ik_bone_3d : bone_list) { + ik_bone_3d->update_default_constraint_transform(); + } + for (int32_t constraint_i = 0; constraint_i < get_constraint_count(); ++constraint_i) { + String constraint_name = get_constraint_name(constraint_i); + twist_constraint_defaults[constraint_name] = get_constraint_twist_transform(constraint_i); + orientation_constraint_defaults[constraint_name] = get_constraint_orientation_transform(constraint_i); + bone_direction_constraint_defaults[constraint_name] = get_bone_direction_transform(constraint_i); + } + } + for (int32_t constraint_i = 0; constraint_i < get_constraint_count(); ++constraint_i) { + String constraint_name = get_constraint_name(constraint_i); + set_constraint_twist_transform(constraint_i, twist_constraint_defaults[constraint_name]); + set_constraint_orientation_transform(constraint_i, orientation_constraint_defaults[constraint_name]); + set_bone_direction_transform(constraint_i, bone_direction_constraint_defaults[constraint_name]); + } + if (queue_debug_skeleton) { + queue_debug_skeleton = false; + } +} + +real_t ManyBoneIK3D::get_pin_weight(int32_t p_pin_index) const { + ERR_FAIL_INDEX_V(p_pin_index, pins.size(), 0.0); + const Ref effector_template = pins[p_pin_index]; + return effector_template->get_weight(); +} + +void ManyBoneIK3D::set_pin_weight(int32_t p_pin_index, const real_t &p_weight) { + ERR_FAIL_INDEX(p_pin_index, pins.size()); + Ref effector_template = pins[p_pin_index]; + if (effector_template.is_null()) { + effector_template.instantiate(); + pins.write[p_pin_index] = effector_template; + } + effector_template->set_weight(p_weight); + set_dirty(); +} + +Vector3 ManyBoneIK3D::get_pin_direction_priorities(int32_t p_pin_index) const { + ERR_FAIL_INDEX_V(p_pin_index, pins.size(), Vector3(0, 0, 0)); + const Ref effector_template = pins[p_pin_index]; + return effector_template->get_direction_priorities(); +} + +void ManyBoneIK3D::set_pin_direction_priorities(int32_t p_pin_index, const Vector3 &p_priority_direction) { + ERR_FAIL_INDEX(p_pin_index, pins.size()); + Ref effector_template = pins[p_pin_index]; + if (effector_template.is_null()) { + effector_template.instantiate(); + pins.write[p_pin_index] = effector_template; + } + effector_template->set_direction_priorities(p_priority_direction); + set_dirty(); +} + +void ManyBoneIK3D::set_dirty() { + is_dirty = true; + is_gizmo_dirty = true; + if (timer.is_valid()) { + timer->set_time_left(0.5f); + } +} + +void ManyBoneIK3D::_on_timer_timeout() { + notify_property_list_changed(); +} + +int32_t ManyBoneIK3D::find_constraint(String p_string) const { + for (int32_t constraint_i = 0; constraint_i < constraint_count; constraint_i++) { + if (get_constraint_name(constraint_i) == p_string) { + return constraint_i; + } + } + return -1; +} + +Skeleton3D *ManyBoneIK3D::get_skeleton() const { + Node *node = get_node_or_null(skeleton_node_path); + if (!node) { + return nullptr; + } + return cast_to(node); +} + +NodePath ManyBoneIK3D::get_skeleton_node_path() { + return skeleton_node_path; +} + +void ManyBoneIK3D::set_skeleton_node_path(NodePath p_skeleton_node_path) { + skeleton_node_path = p_skeleton_node_path; + register_skeleton(); + set_dirty(); // Duplicated for ease of verification. +} + +void ManyBoneIK3D::_notification(int p_what) { + switch (p_what) { + case NOTIFICATION_READY: { + set_process_priority(1); + set_notify_transform(true); + } break; + case NOTIFICATION_ENTER_TREE: { + timer.instantiate(); + timer->set_time_left(0.5); + timer->connect("timeout", callable_mp(this, &ManyBoneIK3D::_on_timer_timeout)); + set_process_internal(true); + } break; + case NOTIFICATION_EXIT_TREE: { + set_process_internal(false); + } break; + case NOTIFICATION_TRANSFORM_CHANGED: { + update_gizmos(); + } break; + case NOTIFICATION_INTERNAL_PROCESS: { + execute(get_process_delta_time()); + update_gizmos(); + } break; + } +} + +void ManyBoneIK3D::remove_constraint(int32_t p_index) { + ERR_FAIL_INDEX(p_index, constraint_count); + + constraint_names.remove_at(p_index); + kusudama_limit_cone_count.remove_at(p_index); + kusudama_limit_cones.remove_at(p_index); + kusudama_twist.remove_at(p_index); + bone_resistance.remove_at(p_index); + + constraint_count--; + + set_dirty(); +} + +void ManyBoneIK3D::_set_bone_count(int32_t p_count) { + bone_damp.resize(p_count); + for (int32_t bone_i = p_count; bone_i-- > bone_count;) { + bone_damp.write[bone_i] = get_default_damp(); + } + bone_count = p_count; +} + +int32_t ManyBoneIK3D::get_bone_count() const { + return bone_count; +} + +Vector> ManyBoneIK3D::get_bone_list() const { + return bone_list; +} + +void ManyBoneIK3D::set_bone_direction_transform(int32_t p_index, Transform3D p_transform) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + if (!get_skeleton()) { + return; + } + String bone_name = constraint_names[p_index]; + int32_t bone_index = get_skeleton()->find_bone(bone_name); + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(bone_index); + if (ik_bone.is_null() || ik_bone->get_constraint().is_null()) { + continue; + } + if (ik_bone->get_bone_direction_transform().is_null()) { + continue; + } + ik_bone->get_bone_direction_transform()->set_transform(p_transform); + break; + } +} + +Transform3D ManyBoneIK3D::get_bone_direction_transform(int32_t p_index) const { + if (p_index < 0 || p_index >= constraint_names.size() || get_skeleton() == nullptr) { + return Transform3D(); + } + + String bone_name = constraint_names[p_index]; + int32_t bone_index = get_skeleton()->find_bone(bone_name); + for (const Ref &segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(bone_index); + if (ik_bone.is_null() || ik_bone->get_constraint().is_null()) { + continue; + } + return ik_bone->get_bone_direction_transform()->get_transform(); + } + return Transform3D(); +} + +Transform3D ManyBoneIK3D::get_constraint_orientation_transform(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, constraint_names.size(), Transform3D()); + String bone_name = constraint_names[p_index]; + if (!segmented_skeletons.size()) { + return Transform3D(); + } + if (!get_skeleton()) { + return Transform3D(); + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + return ik_bone->get_constraint_orientation_transform()->get_transform(); + } + return Transform3D(); +} + +void ManyBoneIK3D::set_constraint_orientation_transform(int32_t p_index, Transform3D p_transform) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + String bone_name = constraint_names[p_index]; + if (!get_skeleton()) { + return; + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + ik_bone->get_constraint_orientation_transform()->set_transform(p_transform); + break; + } +} + +Transform3D ManyBoneIK3D::get_constraint_twist_transform(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, constraint_names.size(), Transform3D()); + String bone_name = constraint_names[p_index]; + if (!segmented_skeletons.size()) { + return Transform3D(); + } + if (!get_skeleton()) { + return Transform3D(); + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + return ik_bone->get_constraint_twist_transform()->get_transform(); + } + return Transform3D(); +} + +void ManyBoneIK3D::set_constraint_twist_transform(int32_t p_index, Transform3D p_transform) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + String bone_name = constraint_names[p_index]; + if (!get_skeleton()) { + return; + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + ik_bone->get_constraint_twist_transform()->set_transform(p_transform); + break; + } +} + +bool ManyBoneIK3D::get_pin_enabled(int32_t p_effector_index) const { + ERR_FAIL_INDEX_V(p_effector_index, pins.size(), false); + Ref effector_template = pins[p_effector_index]; + return !effector_template->get_target_node().is_empty(); +} + +void ManyBoneIK3D::register_skeleton() { + if (!get_pin_count() && !get_constraint_count()) { + reset_constraints(); + } + set_dirty(); +} + +void ManyBoneIK3D::reset_constraints() { + Skeleton3D *skeleton = get_skeleton(); + if (skeleton) { + orientation_constraint_defaults.clear(); + twist_constraint_defaults.clear(); + bone_direction_constraint_defaults.clear(); + int32_t saved_pin_count = get_pin_count(); + set_pin_count(0); + set_pin_count(saved_pin_count); + int32_t saved_constraint_count = constraint_names.size(); + set_constraint_count(0); + set_constraint_count(saved_constraint_count); + _set_bone_count(0); + _set_bone_count(saved_constraint_count); + } + set_dirty(); +} + +bool ManyBoneIK3D::get_constraint_mode() const { + return is_constraint_mode; +} + +void ManyBoneIK3D::set_constraint_mode(bool p_enabled) { + is_constraint_mode = p_enabled; +} + +int32_t ManyBoneIK3D::get_ui_selected_bone() const { + return ui_selected_bone; +} + +void ManyBoneIK3D::set_ui_selected_bone(int32_t p_ui_selected_bone) { + ui_selected_bone = p_ui_selected_bone; +} + +void ManyBoneIK3D::set_stabilization_passes(int32_t p_passes) { + stabilize_passes = p_passes; + set_dirty(); +} + +int32_t ManyBoneIK3D::get_stabilization_passes() { + return stabilize_passes; +} + +void ManyBoneIK3D::set_twist_constraint_defaults(Dictionary p_defaults) { + twist_constraint_defaults = p_defaults; + set_dirty(); +} + +Dictionary ManyBoneIK3D::get_twist_constraint_defaults() { + return twist_constraint_defaults; +} + +void ManyBoneIK3D::set_orientation_constraint_defaults(Dictionary p_defaults) { + orientation_constraint_defaults = p_defaults; +} + +Dictionary ManyBoneIK3D::get_orientation_constraint_defaults() { + return orientation_constraint_defaults; +} + +void ManyBoneIK3D::set_bone_direction_constraint_defaults(Dictionary p_defaults) { + bone_direction_constraint_defaults = p_defaults; + set_dirty(); +} + +Dictionary ManyBoneIK3D::get_bone_direction_constraint_defaults() { + return bone_direction_constraint_defaults; +} + +Transform3D ManyBoneIK3D::get_godot_skeleton_transform_inverse() { + return godot_skeleton_transform_inverse; +} + +Ref ManyBoneIK3D::get_godot_skeleton_transform() { + return godot_skeleton_transform; +} + +void ManyBoneIK3D::set_setup_humanoid_bones(bool set_targets) { + is_setup_humanoid_bones = set_targets; + setup_humanoid_bones(is_setup_humanoid_bones); + set_dirty(); +} + +bool ManyBoneIK3D::get_setup_humanoid_bones() const { + return is_setup_humanoid_bones; +} + +void ManyBoneIK3D::setup_humanoid_bones(bool p_set_targets) { + // **Rotation Twist** + // | Body Part | Description | + // |-----------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| + // | Head | The head can rotate side-to-side up to 60-70 degrees, enabling the character to look left and right. | + // | Neck | The neck can rotate side-to-side up to 50-60 degrees for looking left and right. | + // | [Side]UpperLeg | The upper leg can rotate slightly up to 5-10 degrees for sitting. | + // | [Side]UpperArm | The upper arm can also rotate slightly up to 30-40 degrees for more natural arm movement. | + // | [Side]Hand | The wrist can also rotate slightly up to 20-30 degrees, enabling the hand to twist inward or outward for grasping and gesturing. | + // | Hips | The hips can rotate up to 45-55 degrees, allowing for twisting and turning movements. | + // | Spine | The spine can rotate up to 20-30 degrees, providing flexibility for bending and twisting. | + // | Chest | The chest can rotate up to 15-25 degrees, contributing to the twisting motion of the upper body. | + // | UpperChest | The upper chest can rotate up to 10-20 degrees, aiding in the overall rotation of the torso. + // | [Side]UpperLeg | The upper leg can rotate up to 30-40 degrees, allowing for movements such as kicking or stepping. | + // | [Side]LowerLeg | The lower leg can rotate slightly up to 10-15 degrees, providing flexibility for running or jumping. | + // | [Side]Foot | The foot can rotate inward or outward (inversion and eversion) up to 20-30 degrees, enabling balance and various stances. | + // | [Side]Shoulder | The shoulder can rotate up to 90 degrees in a normal range of motion. This allows for movements such as lifting an arm or throwing. However, with forced movement, it can rotate beyond this limit. | + + // **Rotation Swing** + // | Body Part | Description | + // |-----------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| + // | Hips | The hips can tilt forward and backward up to 20-30 degrees, allowing the legs to swing in a wide arc during walking or running. They can also move side-to-side up to 10-20 degrees, enabling the legs to spread apart or come together. | + // | UpperChest | The upper chest can tilt forward and backward up to 10-20 degrees, allowing for natural breathing and posture adjustments. | + // | Chest | The chest can tilt forward and backward up to 10-20 degrees, allowing for natural breathing and posture adjustments. | + // | Spine | The spine can tilt forward and backward up to 35-45 degrees, allowing for bending and straightening of the torso. | + // | [Side]UpperLeg | The upper leg can swing forward and backward up to 80-90 degrees, allowing for steps during walking and running. | + // | [Side]LowerLeg | The knee can bend and straighten up to 110-120 degrees, allowing the lower leg to move towards or away from the upper leg during walking, running, and stepping. | + // | [Side]Foot | The ankle can tilt up (dorsiflexion) up to 10-20 degrees and down (plantarflexion) up to 35-40 degrees, allowing the foot to step and adjust during walking and running. | + // | [Side]Shoulder | The shoulder can tilt forward and backward up to 160 degrees, allowing the arms to swing in a wide arc. They can also move side-to-side up to 40-50 degrees, enabling the arms to extend outwards or cross over the chest. | + // | [Side]UpperArm | The upper arm can swing forward and backward up to 110-120 degrees, allowing for reaching and swinging motions. | + // | [Side]LowerArm | The elbow can bend and straighten up to 120-130 degrees, allowing the forearm to move towards or away from the upper arm during reaching and swinging motions. | + // | [Side]Hand | The wrist can tilt up and down up to 50-60 degrees, allowing the hand to move towards or away from the forearm. + set_process_thread_group(PROCESS_THREAD_GROUP_SUB_THREAD); + set_process_thread_group_order(100); + Skeleton3D *skeleton = cast_to(get_node_or_null(get_skeleton_node_path())); + ERR_FAIL_NULL(skeleton); + skeleton->set_show_rest_only(true); + skeleton->reset_bone_poses(); + Ref humanoid_profile; + humanoid_profile.instantiate(); + PackedStringArray humanoid_bones; + if (!p_set_targets) { + return; + } + Vector bones = { + "Root", + "Head", + "UpperChest", + "LeftLowerArm", + "LeftHand", + "RightLowerArm", + "RightHand", + "Spine", + "LeftLowerLeg", + "LeftFoot", + "RightLowerLeg", + "RightFoot", + }; + set_pin_count(0); + set_pin_count(bones.size()); + TypedArray children = find_children("*", "Marker3D"); + for (int i = 0; i < children.size(); ++i) { + Node *node = cast_to(children[i]); + node->queue_free(); + } + for (int pin_i = 0; pin_i < bones.size(); pin_i++) { + String bone_name = bones[pin_i]; + Marker3D *marker_3d = memnew(Marker3D); + marker_3d->set_name(bone_name); + add_child(marker_3d, true); + marker_3d->set_owner(get_owner()); + int32_t bone_i = skeleton->find_bone(bone_name); + Transform3D pose = skeleton->get_global_transform().affine_inverse() * skeleton->get_bone_global_pose_no_override(bone_i); + marker_3d->set_global_transform(pose); + set_pin_nodepath(pin_i, get_path_to(marker_3d)); + set_pin_bone_name(pin_i, bone_name); + if (bone_name == "Root") { + continue; + } + set_pin_passthrough_factor(pin_i, 1.0f); + } + skeleton_changed(get_skeleton()); + set_constraint_count(0); + for (int bone_i = 0; bone_i < get_bone_list().size(); bone_i++) { + int32_t matched_bone_i = get_bone_list()[bone_i]->get_bone_id(); + String bone_name = skeleton->get_bone_name(matched_bone_i); + int32_t humanoid_bone_i = humanoid_profile->find_bone(bone_name); + if (humanoid_bone_i == -1) { + continue; + } + bool isFinger = bone_name.ends_with("ThumbMetacarpal") || + bone_name.ends_with("ThumbProximal") || + bone_name.ends_with("ThumbDistal") || + bone_name.ends_with("IndexProximal") || + bone_name.ends_with("IndexIntermediate") || + bone_name.ends_with("IndexDistal") || + bone_name.ends_with("MiddleProximal") || + bone_name.ends_with("MiddleIntermediate") || + bone_name.ends_with("MiddleDistal") || + bone_name.ends_with("RingProximal") || + bone_name.ends_with("RingIntermediate") || + bone_name.ends_with("RingDistal") || + bone_name.ends_with("LittleProximal") || + bone_name.ends_with("LittleIntermediate") || + bone_name.ends_with("LittleDistal"); + if (isFinger) { + continue; + } + SkeletonProfileHumanoidConstraint::BoneConstraint constraint = humanoid_profile->get_bone_constraint(bone_name); + int32_t constraint_i = get_constraint_count(); + add_constraint(); + set_constraint_name(constraint_i, bone_name); + set_kusudama_resistance(constraint_i, 1.0); + } + for (int bone_i = 0; bone_i < humanoid_profile->get_bone_size(); bone_i++) { + String bone_name = humanoid_profile->get_bone_name(bone_i); + SkeletonProfileHumanoidConstraint::BoneConstraint constraint = humanoid_profile->get_bone_constraint(bone_name); + int32_t constraint_i = find_constraint(bone_name); + if (constraint_i == -1) { + continue; + } + Vector cones = constraint.swing_limit_cones; + for (int32_t cone_i = 0; cone_i < cones.size(); cone_i++) { + set_kusudama_limit_cone_count(constraint_i, cones.size()); + set_kusudama_limit_cone(constraint_i, cone_i, cones[cone_i].center, cones[cone_i].radius); + } + set_kusudama_twist_from_range(constraint_i, constraint.twist_from, constraint.twist_range); + set_kusudama_resistance(constraint_i, constraint.resistance); + } + is_setup_humanoid_bones = false; + skeleton->set_show_rest_only(is_setup_humanoid_bones); +} + +void ManyBoneIK3D::set_kusudama_resistance(int32_t p_index, real_t p_resistance) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + String bone_name = constraint_names[p_index]; + bone_resistance.write[p_index] = p_resistance; + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + ik_bone->get_constraint()->set_resistance(p_resistance); + ik_bone->set_skeleton_bone_pose(get_skeleton()); + break; + } + set_dirty(); +} + +real_t ManyBoneIK3D::get_kusudama_resistance(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, constraint_names.size(), 0.0f); + return bone_resistance[p_index]; +} + +void ManyBoneIK3D::add_constraint() { + int32_t old_count = constraint_count; + set_constraint_count(constraint_count + 1); + constraint_names.write[old_count] = String(); + kusudama_limit_cone_count.write[old_count] = 0; + kusudama_limit_cones.write[old_count].resize(1); + kusudama_limit_cones.write[old_count].write[0] = Vector4(0, 1, 0, Math_PI); + kusudama_twist.write[old_count] = Vector2(0, Math_PI); + bone_resistance.write[old_count] = 0.0f; + set_dirty(); +} + +real_t ManyBoneIK3D::get_kusudama_twist_current(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, constraint_names.size(), 0.0f); + String bone_name = constraint_names[p_index]; + if (!segmented_skeletons.size()) { + return 0; + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + return ik_bone->get_constraint()->get_current_twist_rotation(ik_bone->get_ik_transform(), ik_bone->get_bone_direction_transform(), ik_bone->get_constraint_twist_transform()); + } + return 0; +} + +void ManyBoneIK3D::set_kusudama_twist_current(int32_t p_index, real_t p_rotation) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + String bone_name = constraint_names[p_index]; + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + ik_bone->get_constraint()->set_current_twist_rotation(ik_bone->get_ik_transform(), ik_bone->get_bone_direction_transform(), ik_bone->get_constraint_twist_transform(), p_rotation); + ik_bone->set_skeleton_bone_pose(get_skeleton()); + notify_property_list_changed(); + } +} diff --git a/modules/many_bone_ik/src/many_bone_ik_3d.h b/modules/many_bone_ik/src/many_bone_ik_3d.h new file mode 100644 index 000000000000..cf07e0d15bfd --- /dev/null +++ b/modules/many_bone_ik/src/many_bone_ik_3d.h @@ -0,0 +1,201 @@ +/**************************************************************************/ +/* many_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. */ +/**************************************************************************/ + +#ifndef MANY_BONE_IK_3D_H +#define MANY_BONE_IK_3D_H + +#include "core/math/math_defs.h" +#include "core/math/transform_3d.h" +#include "core/math/vector3.h" +#include "ik_bone_3d.h" +#include "ik_effector_template_3d.h" +#include "math/ik_node_3d.h" + +#include "core/object/ref_counted.h" +#include "core/os/memory.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/main/timer.h" +#include "scene/resources/skeleton_profile.h" + +#ifdef TOOLS_ENABLED +#include "editor/editor_node.h" +#include "editor/editor_undo_redo_manager.h" +#endif + +class ManyBoneIK3D : public Node3D { + GDCLASS(ManyBoneIK3D, Node3D); + +private: + Dictionary twist_constraint_defaults, orientation_constraint_defaults, bone_direction_constraint_defaults; + bool is_constraint_mode = false; + NodePath skeleton_path; + Vector> segmented_skeletons; + int32_t constraint_count = 0, pin_count = 0, bone_count = 0; + Vector constraint_names; + Vector> pins; + Vector> bone_list; + Vector kusudama_twist; + Vector bone_damp; + Vector bone_resistance; + Vector> kusudama_limit_cones; + Vector kusudama_limit_cone_count; + float MAX_KUSUDAMA_LIMIT_CONES = 10; + int32_t iterations_per_frame = 10; + float default_damp = Math::deg_to_rad(5.0f); + bool queue_debug_skeleton = false; + Ref godot_skeleton_transform; + Transform3D godot_skeleton_transform_inverse; + Ref ik_origin; + bool is_dirty = true; + NodePath skeleton_node_path = NodePath(".."); + int32_t ui_selected_bone = -1, stabilize_passes = 4; + bool is_gizmo_dirty = false; + bool is_setup_humanoid_bones = false; + Ref timer; + void _on_timer_timeout(); + void update_ik_bones_transform(); + void update_skeleton_bones_transform(); + Vector> get_bone_effectors() const; + void set_pin_bone_name(int32_t p_effector_index, StringName p_name) const; + void set_constraint_name(int32_t p_index, String p_name); + void set_pin_count(int32_t p_value); + void set_constraint_count(int32_t p_count); + void _remove_pin(int32_t p_index); + void _set_bone_count(int32_t p_count); + +protected: + bool _set(const StringName &p_name, const Variant &p_value); + bool _get(const StringName &p_name, Variant &r_ret) const; + void _get_property_list(List *p_list) const; + static void _bind_methods(); + virtual void skeleton_changed(Skeleton3D *skeleton); + virtual void execute(real_t delta); + void _notification(int p_what); + +public: + static Vector3 convert_attitude_azimuth_to_coordinate(float attitude, float azimuth) { + Vector3 p_center; + p_center.x = sin(attitude) * cos(azimuth); + p_center.y = sin(attitude) * sin(azimuth); + p_center.z = cos(attitude); + return p_center; + } + static Vector2 convert_coordinate_to_attitude_azimuth(Vector3 p_center) { + float attitude = acos(CLAMP(p_center.z, -1.0f, 1.0f)); + float azimuth = 0.0f; + + if (ABS(p_center.z) < 1.0f) { + azimuth = atan2(p_center.y, p_center.x); + if (azimuth < 0.0f) { + azimuth += Math_TAU; + } + } else { + azimuth = 0.0f; + } + + return Vector2(attitude, azimuth); + } + void add_constraint(); + void set_stabilization_passes(int32_t p_passes); + int32_t get_stabilization_passes(); + void set_twist_constraint_defaults(Dictionary p_defaults); + Dictionary get_twist_constraint_defaults(); + void set_orientation_constraint_defaults(Dictionary p_defaults); + Dictionary get_orientation_constraint_defaults(); + void set_bone_direction_constraint_defaults(Dictionary p_defaults); + Dictionary get_bone_direction_constraint_defaults(); + Transform3D get_godot_skeleton_transform_inverse(); + Ref get_godot_skeleton_transform(); + void set_ui_selected_bone(int32_t p_ui_selected_bone); + int32_t get_ui_selected_bone() const; + void set_constraint_mode(bool p_enabled); + bool get_constraint_mode() const; + bool get_pin_enabled(int32_t p_effector_index) const; + void set_skeleton_node_path(NodePath p_skeleton_node_path); + void register_skeleton(); + void reset_constraints(); + NodePath get_skeleton_node_path(); + Skeleton3D *get_skeleton() const; + Vector> get_bone_list() const; + Vector> get_segmented_skeletons(); + float get_iterations_per_frame() const; + void set_iterations_per_frame(const float &p_iterations_per_frame); + void queue_print_skeleton(); + int32_t get_pin_count() const; + void remove_constraint(int32_t p_index); + void set_pin_bone(int32_t p_pin_index, const String &p_bone); + StringName get_pin_bone_name(int32_t p_effector_index) const; + void set_pin_nodepath(int32_t p_effector_index, NodePath p_node_path); + NodePath get_pin_nodepath(int32_t p_effector_index) const; + int32_t find_effector_id(StringName p_bone_name); + void set_pin_target_nodepath(int32_t p_effector_index, const NodePath &p_target_node); + void set_pin_weight(int32_t p_pin_index, const real_t &p_weight); + real_t get_pin_weight(int32_t p_pin_index) const; + void set_pin_direction_priorities(int32_t p_pin_index, const Vector3 &p_priority_direction); + Vector3 get_pin_direction_priorities(int32_t p_pin_index) const; + NodePath get_pin_target_nodepath(int32_t p_pin_index); + void set_pin_passthrough_factor(int32_t p_effector_index, const float p_passthrough_factor); + float get_pin_passthrough_factor(int32_t p_effector_index) const; + real_t get_default_damp() const; + void set_default_damp(float p_default_damp); + int32_t find_constraint(String p_string) const; + int32_t get_constraint_count() const; + StringName get_constraint_name(int32_t p_index) const; + void set_kusudama_resistance(int32_t p_index, real_t p_resistance); + real_t get_kusudama_resistance(int32_t p_index) const; + void set_constraint_twist_transform(int32_t p_index, Transform3D p_transform); + Transform3D get_constraint_twist_transform(int32_t p_index) const; + void set_constraint_orientation_transform(int32_t p_index, Transform3D p_transform); + Transform3D get_constraint_orientation_transform(int32_t p_index) const; + void set_bone_direction_transform(int32_t p_index, Transform3D p_transform); + Transform3D get_bone_direction_transform(int32_t p_index) const; + Vector2 get_kusudama_twist(int32_t p_index) const; + void set_kusudama_limit_cone(int32_t p_bone, int32_t p_index, + Vector3 p_center, float p_radius); + Vector3 get_kusudama_limit_cone_center(int32_t p_constraint_index, int32_t p_index) const; + float get_kusudama_limit_cone_radius(int32_t p_constraint_index, int32_t p_index) const; + int32_t get_kusudama_limit_cone_count(int32_t p_constraint_index) const; + int32_t get_bone_count() const; + void set_kusudama_twist_from_range(int32_t p_index, float from, float range); + void set_kusudama_twist(int32_t p_index, Vector2 p_limit); + void set_kusudama_limit_cone_count(int32_t p_constraint_index, int32_t p_count); + void set_kusudama_limit_cone_center(int32_t p_constraint_index, int32_t p_index, Vector3 p_center); + void set_kusudama_limit_cone_radius(int32_t p_constraint_index, int32_t p_index, float p_radius); + ManyBoneIK3D(); + ~ManyBoneIK3D(); + void set_dirty(); + void setup_humanoid_bones(bool set_targets); + void set_setup_humanoid_bones(bool set_targets); + bool get_setup_humanoid_bones() const; + real_t get_kusudama_twist_current(int32_t p_index) const; + void set_kusudama_twist_current(int32_t p_index, real_t p_rotation); +}; + +#endif // MANY_BONE_IK_3D_H diff --git a/modules/many_bone_ik/src/math/ik_node_3d.cpp b/modules/many_bone_ik/src/math/ik_node_3d.cpp new file mode 100644 index 000000000000..70633e795bd0 --- /dev/null +++ b/modules/many_bone_ik/src/math/ik_node_3d.cpp @@ -0,0 +1,157 @@ +/**************************************************************************/ +/* ik_node_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_node_3d.h" + +void IKNode3D::_propagate_transform_changed() { + Vector> to_remove; + + for (Ref transform : children) { + if (transform.is_null()) { + to_remove.push_back(transform); + } else { + transform->_propagate_transform_changed(); + } + } + + for (Ref transform : to_remove) { + children.erase(transform); + } + + dirty |= DIRTY_GLOBAL; +} + +void IKNode3D::_update_local_transform() const { + local_transform.basis = rotation.scaled(scale); + dirty &= ~DIRTY_LOCAL; +} + +void IKNode3D::rotate_local_with_global(const Basis &p_basis, bool p_propagate) { + if (parent.is_null()) { + return; + } + const Basis &new_rot = parent->get_global_transform().basis; + local_transform.basis = new_rot.inverse() * p_basis * new_rot * local_transform.basis; + dirty |= DIRTY_GLOBAL; + if (p_propagate) { + _propagate_transform_changed(); + } +} + +void IKNode3D::set_transform(const Transform3D &p_transform) { + if (local_transform != p_transform) { + local_transform = p_transform; + dirty |= DIRTY_VECTORS; + _propagate_transform_changed(); + } +} + +void IKNode3D::set_global_transform(const Transform3D &p_transform) { + Transform3D xform = parent.is_valid() ? parent->get_global_transform().affine_inverse() * p_transform : p_transform; + local_transform = xform; + dirty |= DIRTY_VECTORS; + _propagate_transform_changed(); +} + +Transform3D IKNode3D::get_transform() const { + if (dirty & DIRTY_LOCAL) { + _update_local_transform(); + } + + return local_transform; +} + +Transform3D IKNode3D::get_global_transform() const { + if (dirty & DIRTY_GLOBAL) { + if (dirty & DIRTY_LOCAL) { + _update_local_transform(); + } + + if (parent.is_valid()) { + global_transform = parent->get_global_transform() * local_transform; + } else { + global_transform = local_transform; + } + + if (disable_scale) { + global_transform.basis.orthogonalize(); + } + + dirty &= ~DIRTY_GLOBAL; + } + + return global_transform; +} + +void IKNode3D::set_disable_scale(bool p_enabled) { + disable_scale = p_enabled; +} + +bool IKNode3D::is_scale_disabled() const { + return disable_scale; +} + +void IKNode3D::set_parent(Ref p_parent) { + if (parent.is_valid()) { + parent->children.erase(this); + } + parent = p_parent; + if (p_parent.is_valid()) { + parent->children.push_back(this); + } + _propagate_transform_changed(); +} + +Ref IKNode3D::get_parent() const { + return parent; +} + +Vector3 IKNode3D::to_local(const Vector3 &p_global) const { + return get_global_transform().affine_inverse().xform(p_global); +} + +Vector3 IKNode3D::to_global(const Vector3 &p_local) const { + return get_global_transform().xform(p_local); +} + +IKNode3D::~IKNode3D() { + cleanup(); +} + +void IKNode3D::_notification(int p_what) { + if (p_what == NOTIFICATION_PREDELETE) { + cleanup(); + } +} +void IKNode3D::cleanup() { + for (Ref &child : children) { + child->set_parent(Ref()); + } +} diff --git a/modules/many_bone_ik/src/math/ik_node_3d.h b/modules/many_bone_ik/src/math/ik_node_3d.h new file mode 100644 index 000000000000..efeea3e13da3 --- /dev/null +++ b/modules/many_bone_ik/src/math/ik_node_3d.h @@ -0,0 +1,101 @@ +/**************************************************************************/ +/* ik_node_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. */ +/**************************************************************************/ + +#ifndef IK_NODE_3D_H +#define IK_NODE_3D_H + +#include "core/templates/list.h" + +#include "core/io/resource.h" +#include "core/math/transform_3d.h" + +class IKNode3D : public RefCounted { + GDCLASS(IKNode3D, RefCounted); + + enum TransformDirty { + DIRTY_NONE = 0, + DIRTY_VECTORS = 1, + DIRTY_LOCAL = 2, + DIRTY_GLOBAL = 4 + }; + + mutable Transform3D global_transform; + mutable Transform3D local_transform; + mutable Basis rotation; + mutable Vector3 scale = Vector3(1, 1, 1); + + mutable int dirty = DIRTY_NONE; + + Ref parent; + List> children; + + bool disable_scale = false; + + void _update_local_transform() const; + +protected: + void _notification(int p_what); + static void _bind_methods() { + ClassDB::bind_method(D_METHOD("_propagate_transform_changed"), &IKNode3D::_propagate_transform_changed); + ClassDB::bind_method(D_METHOD("_update_local_transform"), &IKNode3D::_update_local_transform); + ClassDB::bind_method(D_METHOD("rotate_local_with_global", "p_basis", "p_propagate"), &IKNode3D::rotate_local_with_global, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("set_transform", "p_transform"), &IKNode3D::set_transform); + ClassDB::bind_method(D_METHOD("set_global_transform", "p_transform"), &IKNode3D::set_global_transform); + ClassDB::bind_method(D_METHOD("get_transform"), &IKNode3D::get_transform); + ClassDB::bind_method(D_METHOD("get_global_transform"), &IKNode3D::get_global_transform); + ClassDB::bind_method(D_METHOD("set_disable_scale", "p_enabled"), &IKNode3D::set_disable_scale); + ClassDB::bind_method(D_METHOD("is_scale_disabled"), &IKNode3D::is_scale_disabled); + ClassDB::bind_method(D_METHOD("set_parent", "p_parent"), &IKNode3D::set_parent); + ClassDB::bind_method(D_METHOD("get_parent"), &IKNode3D::get_parent); + ClassDB::bind_method(D_METHOD("to_local", "p_global"), &IKNode3D::to_local); + ClassDB::bind_method(D_METHOD("to_global", "p_local"), &IKNode3D::to_global); + } + +public: + void _propagate_transform_changed(); + void set_transform(const Transform3D &p_transform); + void set_global_transform(const Transform3D &p_transform); + Transform3D get_transform() const; + Transform3D get_global_transform() const; + + void set_disable_scale(bool p_enabled); + bool is_scale_disabled() const; + + void set_parent(Ref p_parent); + Ref get_parent() const; + + Vector3 to_local(const Vector3 &p_global) const; + Vector3 to_global(const Vector3 &p_local) const; + void rotate_local_with_global(const Basis &p_basis, bool p_propagate = false); + void cleanup(); + ~IKNode3D(); +}; + +#endif // IK_NODE_3D_H diff --git a/modules/many_bone_ik/src/math/qcp.cpp b/modules/many_bone_ik/src/math/qcp.cpp new file mode 100644 index 000000000000..f8c4eb3ab0a6 --- /dev/null +++ b/modules/many_bone_ik/src/math/qcp.cpp @@ -0,0 +1,283 @@ +/**************************************************************************/ +/* qcp.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 "qcp.h" + +QCP::QCP(double p_evec_prec, double p_eval_prec) { + eigenvector_precision = p_eval_prec; + eigenvalue_precision = p_evec_prec; +} + +void QCP::set(PackedVector3Array &r_target, PackedVector3Array &r_moved) { + target = r_target; + moved = r_moved; + rmsd_calculated = false; + transformation_calculated = false; + inner_product_calculated = false; +} + +double QCP::get_rmsd() { + if (!rmsd_calculated) { + calculate_rmsd(moved, target); + rmsd_calculated = true; + } + return rmsd; +} + +Quaternion QCP::get_rotation() { + Quaternion result; + if (!transformation_calculated) { + if (!inner_product_calculated) { + inner_product(target, moved); + } + result = calculate_rotation(); + transformation_calculated = true; + } + return result; +} + +void QCP::calculate_rmsd(double r_length) { + rmsd = Math::sqrt(Math::abs(2.0f * (e0 - max_eigenvalue) / r_length)); +} + +Quaternion QCP::calculate_rotation() { + Quaternion result; + + if (moved.size() == 1) { + Vector3 u = moved[0]; + Vector3 v = target[0]; + double norm_product = u.length() * v.length(); + + if (norm_product == 0.0) { + return Quaternion(); + } + + double dot = u.dot(v); + + if (dot < ((2.0e-15 - 1.0) * norm_product)) { + Vector3 w = u.normalized(); + result = Quaternion(-w.x, -w.y, -w.z, 0.0f).normalized(); + } else { + double q0 = Math::sqrt(0.5 * (1.0 + dot / norm_product)); + double coeff = 1.0 / (2.0 * q0 * norm_product); + Vector3 q = v.cross(u).normalized(); + result = Quaternion(-coeff * q.x, -coeff * q.y, -coeff * q.z, q0).normalized(); + } + } else { + double a13 = -sum_xz_minus_zx; + double a14 = sum_xy_minus_yx; + double a21 = sum_yz_minus_zy; + double a22 = sum_xx_minus_yy - sum_zz - max_eigenvalue; + double a23 = sum_xy_plus_yx; + double a24 = sum_xz_plus_zx; + double a31 = a13; + double a32 = a23; + double a33 = sum_yy - sum_xx - sum_zz - max_eigenvalue; + double a34 = sum_yz_plus_zy; + double a41 = a14; + double a42 = a24; + double a43 = a34; + double a44 = sum_zz - sum_xx_plus_yy - max_eigenvalue; + + double a3344_4334 = a33 * a44 - a43 * a34; + double a3244_4234 = a32 * a44 - a42 * a34; + double a3243_4233 = a32 * a43 - a42 * a33; + double a3143_4133 = a31 * a43 - a41 * a33; + double a3144_4134 = a31 * a44 - a41 * a34; + double a3142_4132 = a31 * a42 - a41 * a32; + + double quaternion_w = a22 * a3344_4334 - a23 * a3244_4234 + a24 * a3243_4233; + double quaternion_x = -a21 * a3344_4334 + a23 * a3144_4134 - a24 * a3143_4133; + double quaternion_y = a21 * a3244_4234 - a22 * a3144_4134 + a24 * a3142_4132; + double quaternion_z = -a21 * a3243_4233 + a22 * a3143_4133 - a23 * a3142_4132; + double qsqr = quaternion_w * quaternion_w + quaternion_x * quaternion_x + quaternion_y * quaternion_y + quaternion_z * quaternion_z; + + if (qsqr < eigenvector_precision) { + return Quaternion(); + } + + quaternion_x *= -1; + quaternion_y *= -1; + quaternion_z *= -1; + double min = quaternion_w; + min = quaternion_x < min ? quaternion_x : min; + min = quaternion_y < min ? quaternion_y : min; + min = quaternion_z < min ? quaternion_z : min; + quaternion_w /= min; + quaternion_x /= min; + quaternion_y /= min; + quaternion_z /= min; + + result = Quaternion(quaternion_x, quaternion_y, quaternion_z, quaternion_w).normalized(); + } + + return result; +} + +double QCP::get_rmsd(PackedVector3Array &r_fixed, PackedVector3Array &r_moved) { + set(r_fixed, r_moved); + return get_rmsd(); +} + +void QCP::translate(Vector3 r_translate, PackedVector3Array &r_x) { + for (Vector3 &p : r_x) { + p += r_translate; + } +} + +Vector3 QCP::get_translation() { + return target_center - moved_center; +} + +Vector3 QCP::move_to_weighted_center(PackedVector3Array &r_to_center, Vector &r_weight) { + Vector3 center; + double total_weight = 0; + bool weight_is_empty = r_weight.is_empty(); + int size = r_to_center.size(); + + for (int i = 0; i < size; i++) { + if (!weight_is_empty) { + total_weight += r_weight[i]; + center += r_to_center[i] * r_weight[i]; + } else { + center += r_to_center[i]; + total_weight++; + } + } + + if (total_weight > 0) { + center /= total_weight; + } + + return center; +} + +void QCP::inner_product(PackedVector3Array &coords1, PackedVector3Array &coords2) { + Vector3 weighted_coord1, weighted_coord2; + double sum_of_squares1 = 0, sum_of_squares2 = 0; + + sum_xx = 0; + sum_xy = 0; + sum_xz = 0; + sum_yx = 0; + sum_yy = 0; + sum_yz = 0; + sum_zx = 0; + sum_zy = 0; + sum_zz = 0; + + bool weight_is_empty = weight.is_empty(); + int size = coords1.size(); + + for (int i = 0; i < size; i++) { + if (!weight_is_empty) { + weighted_coord1 = weight[i] * coords1[i]; + sum_of_squares1 += weighted_coord1.dot(coords1[i]); + } else { + weighted_coord1 = coords1[i]; + sum_of_squares1 += weighted_coord1.dot(weighted_coord1); + } + + weighted_coord2 = coords2[i]; + + sum_of_squares2 += weight_is_empty ? weighted_coord2.dot(weighted_coord2) : (weight[i] * weighted_coord2.dot(weighted_coord2)); + + sum_xx += (weighted_coord1.x * weighted_coord2.x); + sum_xy += (weighted_coord1.x * weighted_coord2.y); + sum_xz += (weighted_coord1.x * weighted_coord2.z); + + sum_yx += (weighted_coord1.y * weighted_coord2.x); + sum_yy += (weighted_coord1.y * weighted_coord2.y); + sum_yz += (weighted_coord1.y * weighted_coord2.z); + + sum_zx += (weighted_coord1.z * weighted_coord2.x); + sum_zy += (weighted_coord1.z * weighted_coord2.y); + sum_zz += (weighted_coord1.z * weighted_coord2.z); + } + + double initial_eigenvalue = (sum_of_squares1 + sum_of_squares2) * 0.5; + + sum_xz_plus_zx = sum_xz + sum_zx; + sum_yz_plus_zy = sum_yz + sum_zy; + sum_xy_plus_yx = sum_xy + sum_yx; + sum_yz_minus_zy = sum_yz - sum_zy; + sum_xz_minus_zx = sum_xz - sum_zx; + sum_xy_minus_yx = sum_xy - sum_yx; + sum_xx_plus_yy = sum_xx + sum_yy; + sum_xx_minus_yy = sum_xx - sum_yy; + max_eigenvalue = initial_eigenvalue; + + inner_product_calculated = true; +} + +void QCP::calculate_rmsd(PackedVector3Array &x, PackedVector3Array &y) { + // QCP doesn't handle alignment of single values, so if we only have one point + // we just compute regular distance. + if (x.size() == 1) { + rmsd = x[0].distance_to(y[0]); + rmsd_calculated = true; + } else { + if (!inner_product_calculated) { + inner_product(y, x); + } + calculate_rmsd(w_sum); + } +} + +Quaternion QCP::weighted_superpose(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool translate) { + set(p_moved, p_target, p_weight, translate); + return get_rotation(); +} + +void QCP::set(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool p_translate) { + rmsd_calculated = false; + transformation_calculated = false; + inner_product_calculated = false; + + moved = p_moved; + target = p_target; + weight = p_weight; + + if (p_translate) { + moved_center = move_to_weighted_center(moved, weight); + w_sum = 0; // set wsum to 0 so we don't double up. + target_center = move_to_weighted_center(target, weight); + translate(moved_center * -1, moved); + translate(target_center * -1, target); + } else { + if (!p_weight.is_empty()) { + for (int i = 0; i < p_weight.size(); i++) { + w_sum += p_weight[i]; + } + } else { + w_sum = p_moved.size(); + } + } +} diff --git a/modules/many_bone_ik/src/math/qcp.h b/modules/many_bone_ik/src/math/qcp.h new file mode 100644 index 000000000000..48392f995d9f --- /dev/null +++ b/modules/many_bone_ik/src/math/qcp.h @@ -0,0 +1,104 @@ +/**************************************************************************/ +/* qcp.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. */ +/**************************************************************************/ + +#ifndef QCP_H +#define QCP_H + +#include "core/math/basis.h" +#include "core/math/vector3.h" +#include "core/variant/variant.h" + +/** + * Implementation of the Quaternion-Based Characteristic Polynomial algorithm + * for RMSD and Superposition calculations. + * + * Usage: + * 1. Create a QCP object with two Vector3 arrays of equal length as input. + * The input coordinates are not changed. + * 2. Optionally, provide weighting factors [0 - 1] for each point. + * 3. For maximum efficiency, create a QCP object once and reuse it. + * + * A. Calculate rmsd only: double rmsd = qcp.getRmsd(); + * B. Calculate a 4x4 transformation (Quaternion and translation) matrix: Matrix4f trans = qcp.getTransformationMatrix(); + * C. Get transformed points (y superposed onto the reference x): Vector3[] ySuperposed = qcp.getTransformedCoordinates(); + * + * Citations: + * - Liu P, Agrafiotis DK, & Theobald DL (2011) Reply to comment on: "Fast determination of the optimal Quaternionation matrix for macromolecular superpositions." Journal of Computational Chemistry 32(1):185-186. [http://dx.doi.org/10.1002/jcc.21606] + * - Liu P, Agrafiotis DK, & Theobald DL (2010) "Fast determination of the optimal Quaternionation matrix for macromolecular superpositions." Journal of Computational Chemistry 31(7):1561-1563. [http://dx.doi.org/10.1002/jcc.21439] + * - Douglas L Theobald (2005) "Rapid calculation of RMSDs using a quaternion-based characteristic polynomial." Acta Crystallogr A 61(4):478-480. [http://dx.doi.org/10.1107/S0108767305015266] + * + * This is an adaptation of the original C code QCPQuaternion 1.4 (2012, October 10) to C++. + * The original C source code is available from http://theobald.brandeis.edu/qcp/ and was developed by: + * - Douglas L. Theobald, Department of Biochemistry, Brandeis University + * - Pu Liu, Johnson & Johnson Pharmaceutical Research and Development, L.L.C. + * + * @author Douglas L. Theobald (original C code) + * @author Pu Liu (original C code) + * @author Peter Rose (adapted to Java) + * @author Aleix Lafita (adapted to Java) + * @author Eron Gjoni (adapted to EWB IK) + * @author K. S. Ernest (iFire) Lee (adapted to ManyBoneIK) + */ + +class QCP { + double eigenvector_precision = 1E-6; + double eigenvalue_precision = 1E-11; + + PackedVector3Array target, moved; + Vector weight; + double w_sum = 0; + + Vector3 target_center, moved_center; + + double e0 = 0, rmsd = 0, sum_xy = 0, sum_xz = 0, sum_yx = 0, sum_yz = 0, sum_zx = 0, sum_zy = 0; + double sum_xx_plus_yy = 0, sum_zz = 0, max_eigenvalue = 0, sum_yz_minus_zy = 0, sum_xz_minus_zx = 0, sum_xy_minus_yx = 0; + double sum_xx_minus_yy = 0, sum_xy_plus_yx = 0, sum_xz_plus_zx = 0; + double sum_yy = 0, sum_xx = 0, sum_yz_plus_zy = 0; + bool rmsd_calculated = false, transformation_calculated = false, inner_product_calculated = false; + + void calculate_rmsd(PackedVector3Array &x, PackedVector3Array &y); + void inner_product(PackedVector3Array &coords1, PackedVector3Array &coords2); + void calculate_rmsd(double r_length); + void set(PackedVector3Array &r_target, PackedVector3Array &r_moved); + Quaternion calculate_rotation(); + void set(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool p_translate); + static void translate(Vector3 r_translate, PackedVector3Array &r_x); + double get_rmsd(PackedVector3Array &r_fixed, PackedVector3Array &r_moved); + Vector3 move_to_weighted_center(PackedVector3Array &r_to_center, Vector &r_weight); + +public: + QCP(double p_evec_prec, double p_eval_prec); + double get_rmsd(); + Quaternion weighted_superpose(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool translate); + Quaternion get_rotation(); + Vector3 get_translation(); +}; + +#endif // QCP_H diff --git a/modules/many_bone_ik/src/skeleton_profile_humanoid_constraint.cpp b/modules/many_bone_ik/src/skeleton_profile_humanoid_constraint.cpp new file mode 100644 index 000000000000..dfe650aa4d18 --- /dev/null +++ b/modules/many_bone_ik/src/skeleton_profile_humanoid_constraint.cpp @@ -0,0 +1,139 @@ +/**************************************************************************/ +/* skeleton_profile_humanoid_constraint.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 "skeleton_profile_humanoid_constraint.h" +#include "core/math/math_defs.h" + +SkeletonProfileHumanoidConstraint::SkeletonProfileHumanoidConstraint() { + Vector bone_names = { "Spine", "Chest", "UpperChest", "Hips", "Neck", "Head", "LeftUpperLeg", "RightUpperLeg", "LeftLowerLeg", "RightLowerLeg", "LeftFoot", "RightFoot", "LeftShoulder", "RightShoulder", "LeftUpperArm", "RightUpperArm", "LeftLowerArm", "RightLowerArm", "LeftHand", "RightHand", "LeftThumb", "RightThumb", "LeftEye", "RightEye" }; + for (int i = 0; i < bone_names.size(); ++i) { + StringName bone_name = bone_names[i]; + Vector swing_limit_cones; + int32_t bone_i = find_bone(bone_name); + if (bone_i == -1) { + continue; + } + Transform3D reference_pose = get_reference_pose(bone_i); + Vector3 y_up = reference_pose.basis.get_column(Vector3::AXIS_Y).normalized(); + Vector3 y_up_backwards = y_up; + y_up_backwards.y = -y_up_backwards.y; + float twist_range = Math::deg_to_rad(180.0f); + float twist_from = Math_TAU; + float resistance = 0; + if (bone_name == "Hips") { + twist_from = Math::deg_to_rad(0.0f); + twist_range = Math::deg_to_rad(40.0f); + resistance = 0.5f; + } else if (bone_name == "Spine") { + twist_from = Math::deg_to_rad(10.0f); + twist_range = Math::deg_to_rad(4.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(3.0f))); + resistance = 0.5f; + } else if (bone_name == "Chest") { + twist_from = Math::deg_to_rad(5.0f); + twist_range = Math::deg_to_rad(-10.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(3.0f))); + resistance = 0.5f; + } else if (bone_name == "UpperChest") { + twist_from = Math::deg_to_rad(10.0f); + twist_range = Math::deg_to_rad(40.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(10.0f))); + resistance = 0.6f; + } else if (bone_name == "Neck") { + twist_from = Math::deg_to_rad(15.0f); + twist_range = Math::deg_to_rad(15.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(10.0f))); + resistance = 0.6f; + } else if (bone_name == "Head") { + twist_from = Math::deg_to_rad(15.0f); + twist_range = Math::deg_to_rad(15.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(15.0f))); + resistance = 0.7f; + } else if (bone_name == "LeftUpperLeg") { + twist_from = Math::deg_to_rad(300.0f); + twist_range = Math::deg_to_rad(10.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(25.0f))); + resistance = 0.8f; + } else if (bone_name == "LeftLowerLeg" || bone_name == "RightLowerLeg") { + twist_from = Math::deg_to_rad(180.0f); + twist_range = Math::deg_to_rad(10.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(2.5f))); + swing_limit_cones.push_back(LimitCone(MODEL_REAR, Math::deg_to_rad(2.5f))); + swing_limit_cones.push_back(LimitCone(y_up_backwards, Math::deg_to_rad(2.5f))); + } else if (bone_name == "LeftUpperLeg" || bone_name == "RightUpperLeg") { + twist_from = Math::deg_to_rad(180.0f); + twist_range = Math::deg_to_rad(10.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(25.0f))); + resistance = 0.8f; + } else if (bone_name == "LeftFoot" || bone_name == "RightFoot") { + twist_from = Math::deg_to_rad(180.0f); + twist_range = Math::deg_to_rad(10.0f); + swing_limit_cones.push_back(LimitCone(MODEL_BOTTOM, Math::deg_to_rad(5.0f))); + } else if (bone_name == "LeftShoulder" || bone_name == "RightShoulder") { + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(225.0f))); + } else if (bone_name == "LeftUpperArm" || bone_name == "RightUpperArm") { + twist_from = Math::deg_to_rad(80.0f); + twist_range = Math::deg_to_rad(12.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(90.0f))); + resistance = 0.3; + } else if (bone_name == "LeftLowerArm") { + twist_from = Math::deg_to_rad(-55.0f); + twist_range = Math::deg_to_rad(50.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(2.5f))); + swing_limit_cones.push_back(LimitCone(MODEL_FRONT, Math::deg_to_rad(2.5f))); + swing_limit_cones.push_back(LimitCone(y_up_backwards, Math::deg_to_rad(2.5f))); + resistance = 0.4; + } else if (bone_name == "RightLowerArm") { + twist_from = Math::deg_to_rad(-145.0f); + twist_range = Math::deg_to_rad(50.0f); + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(2.5f))); + swing_limit_cones.push_back(LimitCone(MODEL_FRONT, Math::deg_to_rad(2.5f))); + swing_limit_cones.push_back(LimitCone(y_up_backwards, Math::deg_to_rad(2.5f))); + resistance = 0.4; + } else if (bone_name == "LeftHand" || bone_name == "RightHand") { + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(60.0f))); + } else if (bone_name == "LeftThumb" || bone_name == "RightThumb") { + swing_limit_cones.push_back(LimitCone(y_up, Math::deg_to_rad(90.0f))); + } + set_bone_constraint(bone_name, twist_from, twist_range, swing_limit_cones, resistance); + } +} + +SkeletonProfileHumanoidConstraint::BoneConstraint SkeletonProfileHumanoidConstraint::get_bone_constraint(const StringName &p_bone_name) const { + if (bone_constraints.has(p_bone_name)) { + return bone_constraints[p_bone_name]; + } else { + return BoneConstraint(); + } +} + +void SkeletonProfileHumanoidConstraint::set_bone_constraint(const StringName &p_bone_name, float p_twist_from, float p_twist_range, Vector p_swing_limit_cones, float p_resistance = 0.0f) { + bone_constraints[p_bone_name] = BoneConstraint(p_twist_from, p_twist_range, p_swing_limit_cones, p_resistance); +} diff --git a/modules/many_bone_ik/src/skeleton_profile_humanoid_constraint.h b/modules/many_bone_ik/src/skeleton_profile_humanoid_constraint.h new file mode 100644 index 000000000000..4f8f6f37264b --- /dev/null +++ b/modules/many_bone_ik/src/skeleton_profile_humanoid_constraint.h @@ -0,0 +1,90 @@ +/**************************************************************************/ +/* skeleton_profile_humanoid_constraint.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/resources/skeleton_profile.h" + +class SkeletonProfileHumanoidConstraint : public SkeletonProfileHumanoid { + GDCLASS(SkeletonProfileHumanoidConstraint, SkeletonProfileHumanoid); + +public: + // Unit vector pointing towards the left side of imported 3D assets. + const Vector3 MODEL_LEFT = Vector3(1, 0, 0); + + // Unit vector pointing towards the right side of imported 3D assets. + const Vector3 MODEL_RIGHT = Vector3(-1, 0, 0); + + // Unit vector pointing towards the top side (up) of imported 3D assets. + const Vector3 MODEL_TOP = Vector3(0, 1, 0); + + // Unit vector pointing towards the bottom side (down) of imported 3D assets. + const Vector3 MODEL_BOTTOM = Vector3(0, -1, 0); + + // Unit vector pointing towards the front side (facing forward) of imported 3D assets. + const Vector3 MODEL_FRONT = Vector3(0, 0, 1); + + // Unit vector pointing towards the rear side (facing backwards) of imported 3D assets. + const Vector3 MODEL_REAR = Vector3(0, 0, -1); + + struct LimitCone { + Vector3 center = Vector3(0, 1, 0); + float radius = Math_PI; + + LimitCone(Vector3 p_center = Vector3(), float p_radius = 0) : + center(p_center), radius(p_radius) {} + }; + + struct BoneConstraint { + float twist_from = Math_TAU; + float twist_range = Math_PI; + Vector swing_limit_cones; + float resistance = 0; + + BoneConstraint(float p_twist_from = 0, float p_twist_range = 0, Vector p_swing_limit_cones = Vector(), float p_resistance = 0) : + twist_from(p_twist_from), twist_range(p_twist_range), swing_limit_cones(p_swing_limit_cones), resistance(p_resistance) { + p_swing_limit_cones.resize(10); + } + BoneConstraint(const BoneConstraint &) = default; + BoneConstraint(BoneConstraint &&) = default; + BoneConstraint &operator=(const BoneConstraint &) = default; + BoneConstraint &operator=(BoneConstraint &&) = default; + explicit BoneConstraint(Vector p_swing_limit_cones) : + swing_limit_cones(std::move(p_swing_limit_cones)) {} + }; + void set_bone_constraint(const StringName &p_bone_name, float p_twist_from, float p_twist_range, Vector p_swing_limit_cones, float p_resistance); + BoneConstraint get_bone_constraint(const StringName &p_bone_name) const; + SkeletonProfileHumanoidConstraint(); + ~SkeletonProfileHumanoidConstraint() { + } + +private: + HashMap bone_constraints; +};