diff options
Diffstat (limited to 'scene')
24 files changed, 4793 insertions, 229 deletions
diff --git a/scene/3d/bone_attachment_3d.cpp b/scene/3d/bone_attachment_3d.cpp index 5315e685a0..70361f4787 100644 --- a/scene/3d/bone_attachment_3d.cpp +++ b/scene/3d/bone_attachment_3d.cpp @@ -32,7 +32,15 @@ void BoneAttachment3D::_validate_property(PropertyInfo &property) const { if (property.name == "bone_name") { - Skeleton3D *parent = Object::cast_to<Skeleton3D>(get_parent()); + // Because it is a constant function, we cannot use the _get_skeleton_3d function. + const Skeleton3D *parent = nullptr; + if (use_external_skeleton) { + if (external_skeleton_node_cache.is_valid()) { + parent = Object::cast_to<Skeleton3D>(ObjectDB::get_instance(external_skeleton_node_cache)); + } + } else { + parent = Object::cast_to<Skeleton3D>(get_parent()); + } if (parent) { String names; @@ -52,55 +60,321 @@ void BoneAttachment3D::_validate_property(PropertyInfo &property) const { } } +bool BoneAttachment3D::_set(const StringName &p_path, const Variant &p_value) { + if (p_path == SNAME("override_pose")) { + set_override_pose(p_value); + } else if (p_path == SNAME("override_mode")) { + set_override_mode(p_value); + } else if (p_path == SNAME("use_external_skeleton")) { + set_use_external_skeleton(p_value); + } else if (p_path == SNAME("external_skeleton")) { + set_external_skeleton(p_value); + } + + return true; +} + +bool BoneAttachment3D::_get(const StringName &p_path, Variant &r_ret) const { + if (p_path == SNAME("override_pose")) { + r_ret = get_override_pose(); + } else if (p_path == SNAME("override_mode")) { + r_ret = get_override_mode(); + } else if (p_path == SNAME("use_external_skeleton")) { + r_ret = get_use_external_skeleton(); + } else if (p_path == SNAME("external_skeleton")) { + r_ret = get_external_skeleton(); + } + + return true; +} + +void BoneAttachment3D::_get_property_list(List<PropertyInfo> *p_list) const { + p_list->push_back(PropertyInfo(Variant::BOOL, "override_pose", PROPERTY_HINT_NONE, "")); + if (override_pose) { + p_list->push_back(PropertyInfo(Variant::INT, "override_mode", PROPERTY_HINT_ENUM, "Global Pose Override, Local Pose Override, Custom Pose")); + } + + p_list->push_back(PropertyInfo(Variant::BOOL, "use_external_skeleton", PROPERTY_HINT_NONE, "")); + if (use_external_skeleton) { + p_list->push_back(PropertyInfo(Variant::NODE_PATH, "external_skeleton", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Skeleton3D")); + } +} + +TypedArray<String> BoneAttachment3D::get_configuration_warnings() const { + TypedArray<String> warnings = Node3D::get_configuration_warnings(); + + if (use_external_skeleton) { + if (external_skeleton_node_cache.is_null()) { + warnings.append(TTR("External Skeleton3D node not set! Please set a path to an external Skeleton3D node.")); + } + } else { + Skeleton3D *parent = Object::cast_to<Skeleton3D>(get_parent()); + if (!parent) { + warnings.append(TTR("Parent node is not a Skeleton3D node! Please use an extenral Skeleton3D if you intend to use the BoneAttachment3D without it being a child of a Skeleton3D node.")); + } + } + + if (bone_idx == -1) { + warnings.append(TTR("BoneAttachment3D node is not bound to any bones! Please select a bone to attach this node.")); + } + + return warnings; +} + +void BoneAttachment3D::_update_external_skeleton_cache() { + external_skeleton_node_cache = ObjectID(); + if (has_node(external_skeleton_node)) { + Node *node = get_node(external_skeleton_node); + ERR_FAIL_COND_MSG(!node, "Cannot update external skeleton cache: Node cannot be found!"); + + // Make sure it's a skeleton3D + Skeleton3D *sk = Object::cast_to<Skeleton3D>(node); + ERR_FAIL_COND_MSG(!sk, "Cannot update external skeleton cache: Skeleton3D Nodepath does not point to a Skeleton3D node!"); + + external_skeleton_node_cache = node->get_instance_id(); + } else { + if (external_skeleton_node.is_empty()) { + BoneAttachment3D *parent_attachment = Object::cast_to<BoneAttachment3D>(get_parent()); + if (parent_attachment) { + parent_attachment->_update_external_skeleton_cache(); + if (parent_attachment->has_node(parent_attachment->external_skeleton_node)) { + Node *node = parent_attachment->get_node(parent_attachment->external_skeleton_node); + ERR_FAIL_COND_MSG(!node, "Cannot update external skeleton cache: Parent's Skeleton3D node cannot be found!"); + + // Make sure it's a skeleton3D + Skeleton3D *sk = Object::cast_to<Skeleton3D>(node); + ERR_FAIL_COND_MSG(!sk, "Cannot update external skeleton cache: Parent Skeleton3D Nodepath does not point to a Skeleton3D node!"); + + external_skeleton_node_cache = node->get_instance_id(); + external_skeleton_node = get_path_to(node); + } + } + } + } +} + void BoneAttachment3D::_check_bind() { - Skeleton3D *sk = Object::cast_to<Skeleton3D>(get_parent()); - if (sk) { - int idx = sk->find_bone(bone_name); - if (idx != -1) { - sk->bind_child_node_to_bone(idx, this); - set_transform(sk->get_bone_global_pose(idx)); + Skeleton3D *sk = _get_skeleton3d(); + + if (sk && !bound) { + if (bone_idx <= -1) { + bone_idx = sk->find_bone(bone_name); + } + if (bone_idx != -1) { + sk->call_deferred("connect", "bone_pose_changed", callable_mp(this, &BoneAttachment3D::on_bone_pose_update)); bound = true; + call_deferred(SNAME("on_bone_pose_update"), bone_idx); + } + } +} + +Skeleton3D *BoneAttachment3D::_get_skeleton3d() { + if (use_external_skeleton) { + if (external_skeleton_node_cache.is_valid()) { + return Object::cast_to<Skeleton3D>(ObjectDB::get_instance(external_skeleton_node_cache)); + } else { + _update_external_skeleton_cache(); + if (external_skeleton_node_cache.is_valid()) { + return Object::cast_to<Skeleton3D>(ObjectDB::get_instance(external_skeleton_node_cache)); + } } + } else { + return Object::cast_to<Skeleton3D>(get_parent()); } + return nullptr; } void BoneAttachment3D::_check_unbind() { if (bound) { - Skeleton3D *sk = Object::cast_to<Skeleton3D>(get_parent()); + Skeleton3D *sk = _get_skeleton3d(); + if (sk) { - int idx = sk->find_bone(bone_name); - if (idx != -1) { - sk->unbind_child_node_from_bone(idx, this); - } + sk->disconnect(SNAME("bone_pose_changed"), callable_mp(this, &BoneAttachment3D::on_bone_pose_update)); } bound = false; } } +void BoneAttachment3D::_transform_changed() { + if (!is_inside_tree()) { + return; + } + + if (override_pose) { + Skeleton3D *sk = _get_skeleton3d(); + + ERR_FAIL_COND_MSG(!sk, "Cannot override pose: Skeleton not found!"); + ERR_FAIL_INDEX_MSG(bone_idx, sk->get_bone_count(), "Cannot override pose: Bone index is out of range!"); + + Transform3D our_trans = get_transform(); + if (use_external_skeleton) { + our_trans = sk->world_transform_to_global_pose(get_global_transform()); + } + + if (override_mode == OVERRIDE_MODES::MODE_GLOBAL_POSE) { + sk->set_bone_global_pose_override(bone_idx, our_trans, 1.0, true); + } else if (override_mode == OVERRIDE_MODES::MODE_LOCAL_POSE) { + sk->set_bone_local_pose_override(bone_idx, sk->global_pose_to_local_pose(bone_idx, our_trans), 1.0, true); + } else if (override_mode == OVERRIDE_MODES::MODE_CUSTOM_POSE) { + sk->set_bone_custom_pose(bone_idx, sk->global_pose_to_local_pose(bone_idx, our_trans)); + } + } +} + void BoneAttachment3D::set_bone_name(const String &p_name) { + bone_name = p_name; + Skeleton3D *sk = _get_skeleton3d(); + if (sk) { + set_bone_idx(sk->find_bone(bone_name)); + } +} + +String BoneAttachment3D::get_bone_name() const { + return bone_name; +} + +void BoneAttachment3D::set_bone_idx(const int &p_idx) { if (is_inside_tree()) { _check_unbind(); } - bone_name = p_name; + bone_idx = p_idx; + + Skeleton3D *sk = _get_skeleton3d(); + if (sk) { + if (bone_idx <= -1 || bone_idx >= sk->get_bone_count()) { + WARN_PRINT("Bone index out of range! Cannot connect BoneAttachment to node!"); + bone_idx = -1; + } else { + bone_name = sk->get_bone_name(bone_idx); + } + } if (is_inside_tree()) { _check_bind(); } + + notify_property_list_changed(); } -String BoneAttachment3D::get_bone_name() const { - return bone_name; +int BoneAttachment3D::get_bone_idx() const { + return bone_idx; +} + +void BoneAttachment3D::set_override_pose(bool p_override) { + override_pose = p_override; + set_notify_local_transform(override_pose); + set_process_internal(override_pose); + + if (!override_pose) { + Skeleton3D *sk = _get_skeleton3d(); + if (sk) { + if (override_mode == OVERRIDE_MODES::MODE_GLOBAL_POSE) { + sk->set_bone_global_pose_override(bone_idx, Transform3D(), 0.0, false); + } else if (override_mode == OVERRIDE_MODES::MODE_LOCAL_POSE) { + sk->set_bone_local_pose_override(bone_idx, Transform3D(), 0.0, false); + } else if (override_mode == OVERRIDE_MODES::MODE_CUSTOM_POSE) { + sk->set_bone_custom_pose(bone_idx, Transform3D()); + } + } + _transform_changed(); + } + notify_property_list_changed(); +} + +bool BoneAttachment3D::get_override_pose() const { + return override_pose; +} + +void BoneAttachment3D::set_override_mode(int p_mode) { + if (override_pose) { + Skeleton3D *sk = _get_skeleton3d(); + if (sk) { + if (override_mode == OVERRIDE_MODES::MODE_GLOBAL_POSE) { + sk->set_bone_global_pose_override(bone_idx, Transform3D(), 0.0, false); + } else if (override_mode == OVERRIDE_MODES::MODE_LOCAL_POSE) { + sk->set_bone_local_pose_override(bone_idx, Transform3D(), 0.0, false); + } else if (override_mode == OVERRIDE_MODES::MODE_CUSTOM_POSE) { + sk->set_bone_custom_pose(bone_idx, Transform3D()); + } + } + override_mode = p_mode; + _transform_changed(); + return; + } + override_mode = p_mode; +} + +int BoneAttachment3D::get_override_mode() const { + return override_mode; +} + +void BoneAttachment3D::set_use_external_skeleton(bool p_use_external) { + use_external_skeleton = p_use_external; + + if (use_external_skeleton) { + _check_unbind(); + _update_external_skeleton_cache(); + _check_bind(); + _transform_changed(); + } + + notify_property_list_changed(); +} + +bool BoneAttachment3D::get_use_external_skeleton() const { + return use_external_skeleton; +} + +void BoneAttachment3D::set_external_skeleton(NodePath p_path) { + external_skeleton_node = p_path; + _update_external_skeleton_cache(); + notify_property_list_changed(); +} + +NodePath BoneAttachment3D::get_external_skeleton() const { + return external_skeleton_node; } void BoneAttachment3D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_ENTER_TREE: { + if (use_external_skeleton) { + _update_external_skeleton_cache(); + } _check_bind(); } break; case NOTIFICATION_EXIT_TREE: { _check_unbind(); } break; + case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: { + _transform_changed(); + } break; + case NOTIFICATION_INTERNAL_PROCESS: { + if (_override_dirty) { + _override_dirty = false; + } + } + } +} + +void BoneAttachment3D::on_bone_pose_update(int p_bone_index) { + if (bone_idx == p_bone_index) { + Skeleton3D *sk = _get_skeleton3d(); + if (sk) { + if (!override_pose) { + if (use_external_skeleton) { + set_global_transform(sk->global_pose_to_world_transform(sk->get_bone_global_pose(bone_idx))); + } else { + set_transform(sk->get_bone_global_pose(bone_idx)); + } + } else { + if (!_override_dirty) { + _transform_changed(); + _override_dirty = true; + } + } + } } } @@ -111,5 +385,21 @@ void BoneAttachment3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_bone_name", "bone_name"), &BoneAttachment3D::set_bone_name); ClassDB::bind_method(D_METHOD("get_bone_name"), &BoneAttachment3D::get_bone_name); + ClassDB::bind_method(D_METHOD("set_bone_idx", "bone_idx"), &BoneAttachment3D::set_bone_idx); + ClassDB::bind_method(D_METHOD("get_bone_idx"), &BoneAttachment3D::get_bone_idx); + + ClassDB::bind_method(D_METHOD("on_bone_pose_update", "bone_index"), &BoneAttachment3D::on_bone_pose_update); + + ClassDB::bind_method(D_METHOD("set_override_pose", "override_pose"), &BoneAttachment3D::set_override_pose); + ClassDB::bind_method(D_METHOD("get_override_pose"), &BoneAttachment3D::get_override_pose); + ClassDB::bind_method(D_METHOD("set_override_mode", "override_mode"), &BoneAttachment3D::set_override_mode); + ClassDB::bind_method(D_METHOD("get_override_mode"), &BoneAttachment3D::get_override_mode); + + ClassDB::bind_method(D_METHOD("set_use_external_skeleton", "use_external_skeleton"), &BoneAttachment3D::set_use_external_skeleton); + ClassDB::bind_method(D_METHOD("get_use_external_skeleton"), &BoneAttachment3D::get_use_external_skeleton); + ClassDB::bind_method(D_METHOD("set_external_skeleton", "external_skeleton"), &BoneAttachment3D::set_external_skeleton); + ClassDB::bind_method(D_METHOD("get_external_skeleton"), &BoneAttachment3D::get_external_skeleton); + ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "bone_name"), "set_bone_name", "get_bone_name"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "bone_idx"), "set_bone_idx", "get_bone_idx"); } diff --git a/scene/3d/bone_attachment_3d.h b/scene/3d/bone_attachment_3d.h index 0c6d5f12b1..cf681cace8 100644 --- a/scene/3d/bone_attachment_3d.h +++ b/scene/3d/bone_attachment_3d.h @@ -38,20 +38,59 @@ class BoneAttachment3D : public Node3D { bool bound = false; String bone_name; + int bone_idx = -1; + + bool override_pose = false; + int override_mode = 0; + bool _override_dirty = false; + + enum OVERRIDE_MODES { + MODE_GLOBAL_POSE, + MODE_LOCAL_POSE, + MODE_CUSTOM_POSE + }; + + bool use_external_skeleton = false; + NodePath external_skeleton_node; + ObjectID external_skeleton_node_cache; void _check_bind(); void _check_unbind(); + void _transform_changed(); + void _update_external_skeleton_cache(); + Skeleton3D *_get_skeleton3d(); + protected: virtual void _validate_property(PropertyInfo &property) const override; + bool _get(const StringName &p_path, Variant &r_ret) const; + bool _set(const StringName &p_path, const Variant &p_value); + void _get_property_list(List<PropertyInfo> *p_list) const; void _notification(int p_what); static void _bind_methods(); public: + virtual TypedArray<String> get_configuration_warnings() const override; + void set_bone_name(const String &p_name); String get_bone_name() const; + void set_bone_idx(const int &p_idx); + int get_bone_idx() const; + + void set_override_pose(bool p_override); + bool get_override_pose() const; + void set_override_mode(int p_mode); + int get_override_mode() const; + + void set_use_external_skeleton(bool p_external_skeleton); + bool get_use_external_skeleton() const; + void set_external_skeleton(NodePath p_skeleton); + NodePath get_external_skeleton() const; + + virtual void on_bone_pose_update(int p_bone_index); + BoneAttachment3D(); }; diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp index 12c91271d4..aec0b3002c 100644 --- a/scene/3d/physics_body_3d.cpp +++ b/scene/3d/physics_body_3d.cpp @@ -2199,7 +2199,6 @@ void PhysicalBone3D::_notification(int p_what) { if (parent_skeleton) { if (-1 != bone_id) { parent_skeleton->unbind_physical_bone_from_bone(bone_id); - parent_skeleton->unbind_child_node_from_bone(bone_id, this); bone_id = -1; } } @@ -2656,7 +2655,6 @@ void PhysicalBone3D::update_bone_id() { if (-1 != bone_id) { // Assert the unbind from old node parent_skeleton->unbind_physical_bone_from_bone(bone_id); - parent_skeleton->unbind_child_node_from_bone(bone_id, this); } bone_id = new_bone_id; diff --git a/scene/3d/skeleton_3d.cpp b/scene/3d/skeleton_3d.cpp index c996735761..857916e23d 100644 --- a/scene/3d/skeleton_3d.cpp +++ b/scene/3d/skeleton_3d.cpp @@ -31,7 +31,10 @@ #include "skeleton_3d.h" #include "core/object/message_queue.h" +#include "core/variant/type_info.h" #include "scene/3d/physics_body_3d.h" +#include "scene/resources/skeleton_modification_3d.h" +#include "scene/resources/surface_tool.h" #include "scene/scene_string_names.h" void SkinReference::_skin_changed() { @@ -68,6 +71,13 @@ SkinReference::~SkinReference() { bool Skeleton3D::_set(const StringName &p_path, const Variant &p_value) { String path = p_path; +#ifndef _3D_DISABLED + if (path.begins_with("modification_stack")) { + set_modification_stack(p_value); + return true; + } +#endif //_3D_DISABLED + if (!path.begins_with("bones/")) { return false; } @@ -100,6 +110,13 @@ bool Skeleton3D::_set(const StringName &p_path, const Variant &p_value) { bool Skeleton3D::_get(const StringName &p_path, Variant &r_ret) const { String path = p_path; +#ifndef _3D_DISABLED + if (path.begins_with("modification_stack")) { + r_ret = modification_stack; + return true; + } +#endif //_3D_DISABLED + if (!path.begins_with("bones/")) { return false; } @@ -135,6 +152,14 @@ void Skeleton3D::_get_property_list(List<PropertyInfo> *p_list) const { p_list->push_back(PropertyInfo(Variant::BOOL, prep + "enabled", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR)); p_list->push_back(PropertyInfo(Variant::TRANSFORM3D, prep + "pose", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR)); } + +#ifndef _3D_DISABLED + p_list->push_back( + PropertyInfo(Variant::OBJECT, "modification_stack", + PROPERTY_HINT_RESOURCE_TYPE, + "SkeletonModificationStack3D", + PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_DEFERRED_SET_RESOURCE | PROPERTY_USAGE_DO_NOT_SHARE_ON_DUPLICATE)); +#endif //_3D_DISABLED } void Skeleton3D::_update_process_order() { @@ -145,47 +170,29 @@ void Skeleton3D::_update_process_order() { Bone *bonesptr = bones.ptrw(); int len = bones.size(); - process_order.resize(len); - int *order = process_order.ptrw(); + parentless_bones.clear(); + for (int i = 0; i < len; i++) { if (bonesptr[i].parent >= len) { //validate this just in case ERR_PRINT("Bone " + itos(i) + " has invalid parent: " + itos(bonesptr[i].parent)); bonesptr[i].parent = -1; } - order[i] = i; - bonesptr[i].sort_index = i; - } - //now check process order - int pass_count = 0; - while (pass_count < len * len) { - //using bubblesort because of simplicity, it won't run every frame though. - //bublesort worst case is O(n^2), and this may be an infinite loop if cyclic - bool swapped = false; - for (int i = 0; i < len; i++) { - int parent_idx = bonesptr[order[i]].parent; - if (parent_idx < 0) { - continue; //do nothing because it has no parent - } - //swap indices - int parent_order = bonesptr[parent_idx].sort_index; - if (parent_order > i) { - bonesptr[order[i]].sort_index = parent_order; - bonesptr[parent_idx].sort_index = i; - //swap order - SWAP(order[i], order[parent_order]); - swapped = true; - } - } + bonesptr[i].child_bones.clear(); - if (!swapped) { - break; - } - pass_count++; - } + if (bonesptr[i].parent != -1) { + int parent_bone_idx = bonesptr[i].parent; - if (pass_count == len * len) { - ERR_PRINT("Skeleton3D parenthood graph is cyclic"); + // Check to see if this node is already added to the parent: + if (bonesptr[parent_bone_idx].child_bones.find(i) < 0) { + // Add the child node + bonesptr[parent_bone_idx].child_bones.push_back(i); + } else { + ERR_PRINT("Skeleton3D parenthood graph is cyclic"); + } + } else { + parentless_bones.push_back(i); + } } process_order_dirty = false; @@ -196,78 +203,12 @@ void Skeleton3D::_notification(int p_what) { case NOTIFICATION_UPDATE_SKELETON: { RenderingServer *rs = RenderingServer::get_singleton(); Bone *bonesptr = bones.ptrw(); - int len = bones.size(); - - _update_process_order(); - - const int *order = process_order.ptr(); - - for (int i = 0; i < len; i++) { - Bone &b = bonesptr[order[i]]; - - if (b.disable_rest) { - if (b.enabled) { - Transform3D pose = b.pose; - if (b.custom_pose_enable) { - pose = b.custom_pose * pose; - } - if (b.parent >= 0) { - b.pose_global = bonesptr[b.parent].pose_global * pose; - b.pose_global_no_override = bonesptr[b.parent].pose_global * pose; - } else { - b.pose_global = pose; - b.pose_global_no_override = pose; - } - } else { - if (b.parent >= 0) { - b.pose_global = bonesptr[b.parent].pose_global; - b.pose_global_no_override = bonesptr[b.parent].pose_global; - } else { - b.pose_global = Transform3D(); - b.pose_global_no_override = Transform3D(); - } - } - - } else { - if (b.enabled) { - Transform3D pose = b.pose; - if (b.custom_pose_enable) { - pose = b.custom_pose * pose; - } - if (b.parent >= 0) { - b.pose_global = bonesptr[b.parent].pose_global * (b.rest * pose); - b.pose_global_no_override = bonesptr[b.parent].pose_global * (b.rest * pose); - } else { - b.pose_global = b.rest * pose; - b.pose_global_no_override = b.rest * pose; - } - } else { - if (b.parent >= 0) { - b.pose_global = bonesptr[b.parent].pose_global * b.rest; - b.pose_global_no_override = bonesptr[b.parent].pose_global * b.rest; - } else { - b.pose_global = b.rest; - b.pose_global_no_override = b.rest; - } - } - } - if (b.global_pose_override_amount >= CMP_EPSILON) { - b.pose_global = b.pose_global.interpolate_with(b.global_pose_override, b.global_pose_override_amount); - } - - if (b.global_pose_override_reset) { - b.global_pose_override_amount = 0.0; - } + int len = bones.size(); + dirty = false; - for (const ObjectID &E : b.nodes_bound) { - Object *obj = ObjectDB::get_instance(E); - ERR_CONTINUE(!obj); - Node3D *node_3d = Object::cast_to<Node3D>(obj); - ERR_CONTINUE(!node_3d); - node_3d->set_transform(b.pose_global); - } - } + // Update bone transforms + force_update_all_bone_transforms(); //update skins for (Set<SkinReference *>::Element *E = skin_bindings.front(); E; E = E->next()) { @@ -325,32 +266,53 @@ void Skeleton3D::_notification(int p_what) { } } - dirty = false; - #ifdef TOOLS_ENABLED emit_signal(SceneStringNames::get_singleton()->pose_updated); #endif // TOOLS_ENABLED } break; +#ifndef _3D_DISABLED case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: { // This is active only if the skeleton animates the physical bones // and the state of the bone is not active. - if (animate_physical_bones) { - for (int i = 0; i < bones.size(); i += 1) { - if (bones[i].physical_bone) { - if (bones[i].physical_bone->is_simulating_physics() == false) { - bones[i].physical_bone->reset_to_rest_position(); + if (Engine::get_singleton()->is_editor_hint()) { + if (animate_physical_bones) { + for (int i = 0; i < bones.size(); i += 1) { + if (bones[i].physical_bone) { + if (bones[i].physical_bone->is_simulating_physics() == false) { + bones[i].physical_bone->reset_to_rest_position(); + } } } } } + + if (modification_stack.is_valid()) { + execute_modifications(get_physics_process_delta_time(), SkeletonModificationStack3D::EXECUTION_MODE::execution_mode_physics_process); + } + } break; +#endif // _3D_DISABLED + +#ifndef _3D_DISABLED + case NOTIFICATION_INTERNAL_PROCESS: { + if (modification_stack.is_valid()) { + execute_modifications(get_process_delta_time(), SkeletonModificationStack3D::EXECUTION_MODE::execution_mode_process); + } + } break; +#endif // _3D_DISABLED + +#ifndef _3D_DISABLED case NOTIFICATION_READY: { - if (Engine::get_singleton()->is_editor_hint()) { - set_physics_process_internal(true); + set_physics_process_internal(true); + set_process_internal(true); + + if (modification_stack.is_valid()) { + set_modification_stack(modification_stack); } } break; +#endif // _3D_DISABLED } } @@ -363,15 +325,23 @@ void Skeleton3D::clear_bones_global_pose_override() { } void Skeleton3D::set_bone_global_pose_override(int p_bone, const Transform3D &p_pose, real_t p_amount, bool p_persistent) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); bones.write[p_bone].global_pose_override_amount = p_amount; bones.write[p_bone].global_pose_override = p_pose; bones.write[p_bone].global_pose_override_reset = !p_persistent; _make_dirty(); } +Transform3D Skeleton3D::get_bone_global_pose_override(int p_bone) const { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D()); + return bones[p_bone].global_pose_override; +} + Transform3D Skeleton3D::get_bone_global_pose(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), Transform3D()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D()); if (dirty) { const_cast<Skeleton3D *>(this)->notification(NOTIFICATION_UPDATE_SKELETON); } @@ -379,13 +349,107 @@ Transform3D Skeleton3D::get_bone_global_pose(int p_bone) const { } Transform3D Skeleton3D::get_bone_global_pose_no_override(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), Transform3D()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D()); if (dirty) { const_cast<Skeleton3D *>(this)->notification(NOTIFICATION_UPDATE_SKELETON); } return bones[p_bone].pose_global_no_override; } +void Skeleton3D::clear_bones_local_pose_override() { + for (int i = 0; i < bones.size(); i += 1) { + bones.write[i].local_pose_override_amount = 0; + } + _make_dirty(); +} + +void Skeleton3D::set_bone_local_pose_override(int p_bone, const Transform3D &p_pose, real_t p_amount, bool p_persistent) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); + bones.write[p_bone].local_pose_override_amount = p_amount; + bones.write[p_bone].local_pose_override = p_pose; + bones.write[p_bone].local_pose_override_reset = !p_persistent; + _make_dirty(); +} + +Transform3D Skeleton3D::get_bone_local_pose_override(int p_bone) const { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D()); + return bones[p_bone].local_pose_override; +} + +void Skeleton3D::update_bone_rest_forward_vector(int p_bone, bool p_force_update) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); + + if (bones[p_bone].rest_bone_forward_vector.length_squared() > 0 && p_force_update == false) { + update_bone_rest_forward_axis(p_bone, p_force_update); + } + + // If it is a child/leaf bone... + if (get_bone_parent(p_bone) > 0) { + bones.write[p_bone].rest_bone_forward_vector = bones[p_bone].rest.origin.normalized(); + } else { + // If it has children... + Vector<int> child_bones = get_bone_children(p_bone); + if (child_bones.size() > 0) { + Vector3 combined_child_dir = Vector3(0, 0, 0); + for (int i = 0; i < child_bones.size(); i++) { + combined_child_dir += bones[child_bones[i]].rest.origin.normalized(); + } + combined_child_dir = combined_child_dir / child_bones.size(); + bones.write[p_bone].rest_bone_forward_vector = combined_child_dir.normalized(); + } else { + WARN_PRINT_ONCE("Cannot calculate forward direction for bone " + itos(p_bone)); + WARN_PRINT_ONCE("Assuming direction of (0, 1, 0) for bone"); + bones.write[p_bone].rest_bone_forward_vector = Vector3(0, 1, 0); + } + } + update_bone_rest_forward_axis(p_bone, p_force_update); +} + +void Skeleton3D::update_bone_rest_forward_axis(int p_bone, bool p_force_update) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); + if (bones[p_bone].rest_bone_forward_axis > -1 && p_force_update == false) { + return; + } + + Vector3 forward_axis_absolute = bones[p_bone].rest_bone_forward_vector.abs(); + if (forward_axis_absolute.x > forward_axis_absolute.y && forward_axis_absolute.x > forward_axis_absolute.z) { + if (bones[p_bone].rest_bone_forward_vector.x > 0) { + bones.write[p_bone].rest_bone_forward_axis = BONE_AXIS_X_FORWARD; + } else { + bones.write[p_bone].rest_bone_forward_axis = BONE_AXIS_NEGATIVE_X_FORWARD; + } + } else if (forward_axis_absolute.y > forward_axis_absolute.x && forward_axis_absolute.y > forward_axis_absolute.z) { + if (bones[p_bone].rest_bone_forward_vector.y > 0) { + bones.write[p_bone].rest_bone_forward_axis = BONE_AXIS_Y_FORWARD; + } else { + bones.write[p_bone].rest_bone_forward_axis = BONE_AXIS_NEGATIVE_Y_FORWARD; + } + } else { + if (bones[p_bone].rest_bone_forward_vector.z > 0) { + bones.write[p_bone].rest_bone_forward_axis = BONE_AXIS_Z_FORWARD; + } else { + bones.write[p_bone].rest_bone_forward_axis = BONE_AXIS_NEGATIVE_Z_FORWARD; + } + } +} + +Vector3 Skeleton3D::get_bone_axis_forward_vector(int p_bone) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Vector3(0, 0, 0)); + return bones[p_bone].rest_bone_forward_vector; +} + +int Skeleton3D::get_bone_axis_forward_enum(int p_bone) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, -1); + return bones[p_bone].rest_bone_forward_axis; +} + // skeleton creation api void Skeleton3D::add_bone(const String &p_name) { ERR_FAIL_COND(p_name == "" || p_name.find(":") != -1 || p_name.find("/") != -1); @@ -414,14 +478,15 @@ int Skeleton3D::find_bone(const String &p_name) const { } String Skeleton3D::get_bone_name(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), ""); - + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, ""); return bones[p_bone].name; } void Skeleton3D::set_bone_name(int p_bone, const String &p_name) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); - for (int i = 0; i < bones.size(); i++) { + for (int i = 0; i < bone_size; i++) { if (i != p_bone) { ERR_FAIL_COND(bones[i].name == p_name); } @@ -449,7 +514,8 @@ int Skeleton3D::get_bone_count() const { } void Skeleton3D::set_bone_parent(int p_bone, int p_parent) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); ERR_FAIL_COND(p_parent != -1 && (p_parent < 0)); bones.write[p_bone].parent = p_parent; @@ -458,7 +524,8 @@ void Skeleton3D::set_bone_parent(int p_bone, int p_parent) { } void Skeleton3D::unparent_bone_and_rest(int p_bone) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); _update_process_order(); @@ -475,76 +542,93 @@ void Skeleton3D::unparent_bone_and_rest(int p_bone) { } void Skeleton3D::set_bone_disable_rest(int p_bone, bool p_disable) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); bones.write[p_bone].disable_rest = p_disable; } bool Skeleton3D::is_bone_rest_disabled(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), false); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, false); return bones[p_bone].disable_rest; } int Skeleton3D::get_bone_parent(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), -1); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, -1); return bones[p_bone].parent; } -void Skeleton3D::set_bone_rest(int p_bone, const Transform3D &p_rest) { - ERR_FAIL_INDEX(p_bone, bones.size()); +Vector<int> Skeleton3D::get_bone_children(int p_bone) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Vector<int>()); + return bones[p_bone].child_bones; +} - bones.write[p_bone].rest = p_rest; +void Skeleton3D::set_bone_children(int p_bone, Vector<int> p_children) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); + bones.write[p_bone].child_bones = p_children; + + process_order_dirty = true; _make_dirty(); } -Transform3D Skeleton3D::get_bone_rest(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), Transform3D()); - return bones[p_bone].rest; +void Skeleton3D::add_bone_child(int p_bone, int p_child) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); + bones.write[p_bone].child_bones.push_back(p_child); + + process_order_dirty = true; + _make_dirty(); } -void Skeleton3D::set_bone_enabled(int p_bone, bool p_enabled) { - ERR_FAIL_INDEX(p_bone, bones.size()); +void Skeleton3D::remove_bone_child(int p_bone, int p_child) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); - bones.write[p_bone].enabled = p_enabled; + int child_idx = bones[p_bone].child_bones.find(p_child); + if (child_idx >= 0) { + bones.write[p_bone].child_bones.remove(child_idx); + } else { + WARN_PRINT("Cannot remove child bone: Child bone not found."); + } + + process_order_dirty = true; _make_dirty(); } -bool Skeleton3D::is_bone_enabled(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), false); - return bones[p_bone].enabled; +Vector<int> Skeleton3D::get_parentless_bones() { + return parentless_bones; } -void Skeleton3D::bind_child_node_to_bone(int p_bone, Node *p_node) { - ERR_FAIL_NULL(p_node); - ERR_FAIL_INDEX(p_bone, bones.size()); - - ObjectID id = p_node->get_instance_id(); +void Skeleton3D::set_bone_rest(int p_bone, const Transform3D &p_rest) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); - for (const ObjectID &E : bones[p_bone].nodes_bound) { - if (E == id) { - return; // already here - } - } + bones.write[p_bone].rest = p_rest; + _make_dirty(); +} +Transform3D Skeleton3D::get_bone_rest(int p_bone) const { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D()); - bones.write[p_bone].nodes_bound.push_back(id); + return bones[p_bone].rest; } -void Skeleton3D::unbind_child_node_from_bone(int p_bone, Node *p_node) { - ERR_FAIL_NULL(p_node); - ERR_FAIL_INDEX(p_bone, bones.size()); +void Skeleton3D::set_bone_enabled(int p_bone, bool p_enabled) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); - ObjectID id = p_node->get_instance_id(); - bones.write[p_bone].nodes_bound.erase(id); + bones.write[p_bone].enabled = p_enabled; + _make_dirty(); } -void Skeleton3D::get_bound_child_nodes_to_bone(int p_bone, List<Node *> *p_bound) const { - ERR_FAIL_INDEX(p_bone, bones.size()); - - for (const ObjectID &E : bones[p_bone].nodes_bound) { - Object *obj = ObjectDB::get_instance(E); - ERR_CONTINUE(!obj); - p_bound->push_back(Object::cast_to<Node>(obj)); - } +bool Skeleton3D::is_bone_enabled(int p_bone) const { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, false); + return bones[p_bone].enabled; } void Skeleton3D::clear_bones() { @@ -557,7 +641,8 @@ void Skeleton3D::clear_bones() { // posing api void Skeleton3D::set_bone_pose(int p_bone, const Transform3D &p_pose) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); bones.write[p_bone].pose = p_pose; if (is_inside_tree()) { @@ -565,12 +650,14 @@ void Skeleton3D::set_bone_pose(int p_bone, const Transform3D &p_pose) { } } Transform3D Skeleton3D::get_bone_pose(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), Transform3D()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D()); return bones[p_bone].pose; } void Skeleton3D::set_bone_custom_pose(int p_bone, const Transform3D &p_custom_pose) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); //ERR_FAIL_COND( !is_inside_scene() ); bones.write[p_bone].custom_pose_enable = (p_custom_pose != Transform3D()); @@ -580,7 +667,8 @@ void Skeleton3D::set_bone_custom_pose(int p_bone, const Transform3D &p_custom_po } Transform3D Skeleton3D::get_bone_custom_pose(int p_bone) const { - ERR_FAIL_INDEX_V(p_bone, bones.size(), Transform3D()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D()); return bones[p_bone].custom_pose; } @@ -593,24 +681,22 @@ void Skeleton3D::_make_dirty() { dirty = true; } -int Skeleton3D::get_process_order(int p_idx) { - ERR_FAIL_INDEX_V(p_idx, bones.size(), -1); +void Skeleton3D::localize_rests() { _update_process_order(); - return process_order[p_idx]; -} -Vector<int> Skeleton3D::get_bone_process_orders() { - _update_process_order(); - return process_order; -} + Vector<int> bones_to_process = get_parentless_bones(); + while (bones_to_process.size() > 0) { + int current_bone_idx = bones_to_process[0]; + bones_to_process.erase(current_bone_idx); -void Skeleton3D::localize_rests() { - _update_process_order(); + if (bones[current_bone_idx].parent >= 0) { + set_bone_rest(current_bone_idx, bones[bones[current_bone_idx].parent].rest.affine_inverse() * bones[current_bone_idx].rest); + } - for (int i = bones.size() - 1; i >= 0; i--) { - int idx = process_order[i]; - if (bones[idx].parent >= 0) { - set_bone_rest(idx, bones[bones[idx].parent].rest.affine_inverse() * bones[idx].rest); + // Add the bone's children to the list of bones to be processed + int child_bone_size = bones[current_bone_idx].child_bones.size(); + for (int i = 0; i < child_bone_size; i++) { + bones_to_process.push_back(bones[current_bone_idx].child_bones[i]); } } } @@ -637,7 +723,8 @@ bool Skeleton3D::get_animate_physical_bones() const { } void Skeleton3D::bind_physical_bone_to_bone(int p_bone, PhysicalBone3D *p_physical_bone) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); ERR_FAIL_COND(bones[p_bone].physical_bone); ERR_FAIL_COND(!p_physical_bone); bones.write[p_bone].physical_bone = p_physical_bone; @@ -646,20 +733,23 @@ void Skeleton3D::bind_physical_bone_to_bone(int p_bone, PhysicalBone3D *p_physic } void Skeleton3D::unbind_physical_bone_from_bone(int p_bone) { - ERR_FAIL_INDEX(p_bone, bones.size()); + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone, bone_size); bones.write[p_bone].physical_bone = nullptr; _rebuild_physical_bones_cache(); } PhysicalBone3D *Skeleton3D::get_physical_bone(int p_bone) { - ERR_FAIL_INDEX_V(p_bone, bones.size(), nullptr); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, nullptr); return bones[p_bone].physical_bone; } PhysicalBone3D *Skeleton3D::get_physical_bone_parent(int p_bone) { - ERR_FAIL_INDEX_V(p_bone, bones.size(), nullptr); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, nullptr); if (bones[p_bone].cache_parent_physical_bone) { return bones[p_bone].cache_parent_physical_bone; @@ -669,7 +759,8 @@ PhysicalBone3D *Skeleton3D::get_physical_bone_parent(int p_bone) { } PhysicalBone3D *Skeleton3D::_get_physical_bone_parent(int p_bone) { - ERR_FAIL_INDEX_V(p_bone, bones.size(), nullptr); + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone, bone_size, nullptr); const int parent_bone = bones[p_bone].parent; if (0 > parent_bone) { @@ -800,15 +891,20 @@ Ref<SkinReference> Skeleton3D::register_skin(const Ref<Skin> &p_skin) { // pose changed, rebuild cache of inverses const Bone *bonesptr = bones.ptr(); int len = bones.size(); - const int *order = process_order.ptr(); // calculate global rests and invert them - for (int i = 0; i < len; i++) { - const Bone &b = bonesptr[order[i]]; + Vector<int> bones_to_process = get_parentless_bones(); + while (bones_to_process.size() > 0) { + int current_bone_idx = bones_to_process[0]; + bones_to_process.erase(current_bone_idx); + const Bone &b = bonesptr[current_bone_idx]; + + // Note: the code below may not work by default. May need to track an integer for the bone pose index order + // in the while loop, instead of using current_bone_idx. if (b.parent >= 0) { - skin->set_bind_pose(order[i], skin->get_bind_pose(b.parent) * b.rest); + skin->set_bind_pose(current_bone_idx, skin->get_bind_pose(b.parent) * b.rest); } else { - skin->set_bind_pose(order[i], b.rest); + skin->set_bind_pose(current_bone_idx, b.rest); } } @@ -839,17 +935,202 @@ Ref<SkinReference> Skeleton3D::register_skin(const Ref<Skin> &p_skin) { return skin_ref; } +void Skeleton3D::force_update_all_bone_transforms() { + _update_process_order(); + + for (int i = 0; i < parentless_bones.size(); i++) { + force_update_bone_children_transforms(parentless_bones[i]); + } +} + +void Skeleton3D::force_update_bone_children_transforms(int p_bone_idx) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX(p_bone_idx, bone_size); + + Bone *bonesptr = bones.ptrw(); + List<int> bones_to_process = List<int>(); + bones_to_process.push_back(p_bone_idx); + + while (bones_to_process.size() > 0) { + int current_bone_idx = bones_to_process[0]; + bones_to_process.erase(current_bone_idx); + + Bone &b = bonesptr[current_bone_idx]; + + if (b.disable_rest) { + if (b.enabled) { + Transform3D pose = b.pose; + if (b.custom_pose_enable) { + pose = b.custom_pose * pose; + } + if (b.parent >= 0) { + b.pose_global = bonesptr[b.parent].pose_global * pose; + b.pose_global_no_override = b.pose_global; + } else { + b.pose_global = pose; + b.pose_global_no_override = b.pose_global; + } + } else { + if (b.parent >= 0) { + b.pose_global = bonesptr[b.parent].pose_global; + b.pose_global_no_override = b.pose_global; + } else { + b.pose_global = Transform3D(); + b.pose_global_no_override = b.pose_global; + } + } + + } else { + if (b.enabled) { + Transform3D pose = b.pose; + if (b.custom_pose_enable) { + pose = b.custom_pose * pose; + } + if (b.parent >= 0) { + b.pose_global = bonesptr[b.parent].pose_global * (b.rest * pose); + b.pose_global_no_override = b.pose_global; + } else { + b.pose_global = b.rest * pose; + b.pose_global_no_override = b.pose_global; + } + } else { + if (b.parent >= 0) { + b.pose_global = bonesptr[b.parent].pose_global * b.rest; + b.pose_global_no_override = b.pose_global; + } else { + b.pose_global = b.rest; + b.pose_global_no_override = b.pose_global; + } + } + } + + if (b.local_pose_override_amount >= CMP_EPSILON) { + Transform3D override_local_pose; + if (b.parent >= 0) { + override_local_pose = bonesptr[b.parent].pose_global * (b.rest * b.local_pose_override); + } else { + override_local_pose = (b.rest * b.local_pose_override); + } + b.pose_global = b.pose_global.interpolate_with(override_local_pose, b.local_pose_override_amount); + } + + if (b.global_pose_override_amount >= CMP_EPSILON) { + b.pose_global = b.pose_global.interpolate_with(b.global_pose_override, b.global_pose_override_amount); + } + + if (b.local_pose_override_reset) { + b.local_pose_override_amount = 0.0; + } + if (b.global_pose_override_reset) { + b.global_pose_override_amount = 0.0; + } + + // Add the bone's children to the list of bones to be processed + int child_bone_size = b.child_bones.size(); + for (int i = 0; i < child_bone_size; i++) { + bones_to_process.push_back(b.child_bones[i]); + } + + emit_signal(SceneStringNames::get_singleton()->bone_pose_changed, current_bone_idx); + } +} + // helper functions -Transform3D Skeleton3D::bone_transform_to_world_transform(Transform3D p_bone_transform) { - return get_global_transform() * p_bone_transform; + +Transform3D Skeleton3D::global_pose_to_world_transform(Transform3D p_global_pose) { + return get_global_transform() * p_global_pose; } -Transform3D Skeleton3D::world_transform_to_bone_transform(Transform3D p_world_transform) { +Transform3D Skeleton3D::world_transform_to_global_pose(Transform3D p_world_transform) { return get_global_transform().affine_inverse() * p_world_transform; } +Transform3D Skeleton3D::global_pose_to_local_pose(int p_bone_idx, Transform3D p_global_pose) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone_idx, bone_size, Transform3D()); + if (bones[p_bone_idx].parent >= 0) { + int parent_bone_idx = bones[p_bone_idx].parent; + Transform3D conversion_transform = (bones[parent_bone_idx].pose_global * bones[p_bone_idx].rest); + return conversion_transform.affine_inverse() * p_global_pose; + } else { + return p_global_pose; + } +} + +Transform3D Skeleton3D::local_pose_to_global_pose(int p_bone_idx, Transform3D p_local_pose) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone_idx, bone_size, Transform3D()); + if (bones[p_bone_idx].parent >= 0) { + int parent_bone_idx = bones[p_bone_idx].parent; + Transform3D conversion_transform = (bones[parent_bone_idx].pose_global * bones[p_bone_idx].rest); + return conversion_transform * p_local_pose; + } else { + return p_local_pose; + } +} + +Basis Skeleton3D::global_pose_z_forward_to_bone_forward(int p_bone_idx, Basis p_basis) { + const int bone_size = bones.size(); + ERR_FAIL_INDEX_V(p_bone_idx, bone_size, Basis()); + Basis return_basis = p_basis; + + if (bones[p_bone_idx].rest_bone_forward_axis < 0) { + update_bone_rest_forward_vector(p_bone_idx, true); + } + + if (bones[p_bone_idx].rest_bone_forward_axis == BONE_AXIS_X_FORWARD) { + return_basis.rotate_local(Vector3(0, 1, 0), (Math_PI / 2.0)); + } else if (bones[p_bone_idx].rest_bone_forward_axis == BONE_AXIS_NEGATIVE_X_FORWARD) { + return_basis.rotate_local(Vector3(0, 1, 0), -(Math_PI / 2.0)); + } else if (bones[p_bone_idx].rest_bone_forward_axis == BONE_AXIS_Y_FORWARD) { + return_basis.rotate_local(Vector3(1, 0, 0), -(Math_PI / 2.0)); + } else if (bones[p_bone_idx].rest_bone_forward_axis == BONE_AXIS_NEGATIVE_Y_FORWARD) { + return_basis.rotate_local(Vector3(1, 0, 0), (Math_PI / 2.0)); + } else if (bones[p_bone_idx].rest_bone_forward_axis == BONE_AXIS_Z_FORWARD) { + // Do nothing! + } else if (bones[p_bone_idx].rest_bone_forward_axis == BONE_AXIS_NEGATIVE_Z_FORWARD) { + return_basis.rotate_local(Vector3(0, 0, 1), Math_PI); + } + + return return_basis; +} + +// Modifications + +#ifndef _3D_DISABLED + +void Skeleton3D::set_modification_stack(Ref<SkeletonModificationStack3D> p_stack) { + if (modification_stack.is_valid()) { + modification_stack->is_setup = false; + modification_stack->set_skeleton(nullptr); + } + + modification_stack = p_stack; + if (modification_stack.is_valid()) { + modification_stack->set_skeleton(this); + modification_stack->setup(); + } +} +Ref<SkeletonModificationStack3D> Skeleton3D::get_modification_stack() { + return modification_stack; +} + +void Skeleton3D::execute_modifications(real_t p_delta, int p_execution_mode) { + if (!modification_stack.is_valid()) { + return; + } + + // Needed to avoid the issue where the stack looses reference to the skeleton when the scene is saved. + if (modification_stack->skeleton != this) { + modification_stack->set_skeleton(this); + } + + modification_stack->execute(p_delta, p_execution_mode); +} + +#endif // _3D_DISABLED + void Skeleton3D::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_bone_process_orders"), &Skeleton3D::get_bone_process_orders); ClassDB::bind_method(D_METHOD("add_bone", "name"), &Skeleton3D::add_bone); ClassDB::bind_method(D_METHOD("find_bone", "name"), &Skeleton3D::find_bone); ClassDB::bind_method(D_METHOD("get_bone_name", "bone_idx"), &Skeleton3D::get_bone_name); @@ -862,6 +1143,13 @@ void Skeleton3D::_bind_methods() { ClassDB::bind_method(D_METHOD("unparent_bone_and_rest", "bone_idx"), &Skeleton3D::unparent_bone_and_rest); + ClassDB::bind_method(D_METHOD("get_bone_children", "bone_idx"), &Skeleton3D::get_bone_children); + ClassDB::bind_method(D_METHOD("set_bone_children", "bone_idx", "bone_children"), &Skeleton3D::set_bone_children); + ClassDB::bind_method(D_METHOD("add_bone_child", "bone_idx", "child_bone_idx"), &Skeleton3D::add_bone_child); + ClassDB::bind_method(D_METHOD("remove_bone_child", "bone_idx", "child_bone_idx"), &Skeleton3D::remove_bone_child); + + ClassDB::bind_method(D_METHOD("get_parentless_bones"), &Skeleton3D::get_parentless_bones); + ClassDB::bind_method(D_METHOD("get_bone_rest", "bone_idx"), &Skeleton3D::get_bone_rest); ClassDB::bind_method(D_METHOD("set_bone_rest", "bone_idx", "rest"), &Skeleton3D::set_bone_rest); @@ -879,14 +1167,26 @@ void Skeleton3D::_bind_methods() { ClassDB::bind_method(D_METHOD("clear_bones_global_pose_override"), &Skeleton3D::clear_bones_global_pose_override); ClassDB::bind_method(D_METHOD("set_bone_global_pose_override", "bone_idx", "pose", "amount", "persistent"), &Skeleton3D::set_bone_global_pose_override, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("get_bone_global_pose_override", "bone_idx"), &Skeleton3D::get_bone_global_pose_override); ClassDB::bind_method(D_METHOD("get_bone_global_pose", "bone_idx"), &Skeleton3D::get_bone_global_pose); ClassDB::bind_method(D_METHOD("get_bone_global_pose_no_override", "bone_idx"), &Skeleton3D::get_bone_global_pose_no_override); + ClassDB::bind_method(D_METHOD("clear_bones_local_pose_override"), &Skeleton3D::clear_bones_local_pose_override); + ClassDB::bind_method(D_METHOD("set_bone_local_pose_override", "bone_idx", "pose", "amount", "persistent"), &Skeleton3D::set_bone_local_pose_override, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("get_bone_local_pose_override", "bone_idx"), &Skeleton3D::get_bone_local_pose_override); + ClassDB::bind_method(D_METHOD("get_bone_custom_pose", "bone_idx"), &Skeleton3D::get_bone_custom_pose); ClassDB::bind_method(D_METHOD("set_bone_custom_pose", "bone_idx", "custom_pose"), &Skeleton3D::set_bone_custom_pose); - ClassDB::bind_method(D_METHOD("bone_transform_to_world_transform", "bone_transform"), &Skeleton3D::bone_transform_to_world_transform); - ClassDB::bind_method(D_METHOD("world_transform_to_bone_transform", "world_transform"), &Skeleton3D::world_transform_to_bone_transform); + ClassDB::bind_method(D_METHOD("force_update_all_bone_transforms"), &Skeleton3D::force_update_all_bone_transforms); + ClassDB::bind_method(D_METHOD("force_update_bone_child_transform", "bone_idx"), &Skeleton3D::force_update_bone_children_transforms); + + // Helper functions + ClassDB::bind_method(D_METHOD("global_pose_to_world_transform", "global_pose"), &Skeleton3D::global_pose_to_world_transform); + ClassDB::bind_method(D_METHOD("world_transform_to_global_pose", "world_transform"), &Skeleton3D::world_transform_to_global_pose); + ClassDB::bind_method(D_METHOD("global_pose_to_local_pose", "bone_idx", "global_pose"), &Skeleton3D::global_pose_to_local_pose); + ClassDB::bind_method(D_METHOD("local_pose_to_global_pose", "bone_idx", "local_pose"), &Skeleton3D::local_pose_to_global_pose); + ClassDB::bind_method(D_METHOD("global_pose_z_forward_to_bone_forward", "bone_idx", "basis"), &Skeleton3D::global_pose_z_forward_to_bone_forward); ClassDB::bind_method(D_METHOD("set_animate_physical_bones"), &Skeleton3D::set_animate_physical_bones); ClassDB::bind_method(D_METHOD("get_animate_physical_bones"), &Skeleton3D::get_animate_physical_bones); @@ -896,12 +1196,21 @@ void Skeleton3D::_bind_methods() { ClassDB::bind_method(D_METHOD("physical_bones_add_collision_exception", "exception"), &Skeleton3D::physical_bones_add_collision_exception); ClassDB::bind_method(D_METHOD("physical_bones_remove_collision_exception", "exception"), &Skeleton3D::physical_bones_remove_collision_exception); + // Modifications + ClassDB::bind_method(D_METHOD("set_modification_stack", "modification_stack"), &Skeleton3D::set_modification_stack); + ClassDB::bind_method(D_METHOD("get_modification_stack"), &Skeleton3D::get_modification_stack); + ClassDB::bind_method(D_METHOD("execute_modifications", "delta", "execution_mode"), &Skeleton3D::execute_modifications); + +#ifndef _3D_DISABLED ADD_PROPERTY(PropertyInfo(Variant::BOOL, "animate_physical_bones"), "set_animate_physical_bones", "get_animate_physical_bones"); +#endif // _3D_DISABLED #ifdef TOOLS_ENABLED ADD_SIGNAL(MethodInfo("pose_updated")); #endif // TOOLS_ENABLED + ADD_SIGNAL(MethodInfo("bone_pose_changed", PropertyInfo(Variant::INT, "bone_idx"))); + BIND_CONSTANT(NOTIFICATION_UPDATE_SKELETON); } diff --git a/scene/3d/skeleton_3d.h b/scene/3d/skeleton_3d.h index bccad97d5f..c8a19db813 100644 --- a/scene/3d/skeleton_3d.h +++ b/scene/3d/skeleton_3d.h @@ -32,6 +32,7 @@ #define SKELETON_3D_H #include "scene/3d/node_3d.h" +#include "scene/resources/skeleton_modification_3d.h" #include "scene/resources/skin.h" typedef int BoneId; @@ -61,6 +62,8 @@ public: ~SkinReference(); }; +class SkeletonModificationStack3D; + class Skeleton3D : public Node3D { GDCLASS(Skeleton3D, Node3D); @@ -70,9 +73,8 @@ private: struct Bone { String name; - bool enabled = true; - int parent = -1; - int sort_index = 0; //used for re-sorting process order + bool enabled; + int parent; bool disable_rest = false; Transform3D rest; @@ -91,7 +93,35 @@ private: PhysicalBone3D *physical_bone = nullptr; PhysicalBone3D *cache_parent_physical_bone = nullptr; - List<ObjectID> nodes_bound; + real_t local_pose_override_amount; + bool local_pose_override_reset; + Transform3D local_pose_override; + + Vector<int> child_bones; + + // The forward direction vector and rest bone forward axis are cached because they do not change + // 99% of the time, but recalculating them can be expensive on models with many bones. + Vector3 rest_bone_forward_vector; + int rest_bone_forward_axis = -1; + + Bone() { + parent = -1; + enabled = true; + disable_rest = false; + custom_pose_enable = false; + global_pose_override_amount = 0; + global_pose_override_reset = false; +#ifndef _3D_DISABLED + physical_bone = nullptr; + cache_parent_physical_bone = nullptr; +#endif // _3D_DISABLED + local_pose_override_amount = 0; + local_pose_override_reset = false; + child_bones = Vector<int>(); + + rest_bone_forward_vector = Vector3(0, 0, 0); + rest_bone_forward_axis = -1; + } }; Set<SkinReference *> skin_bindings; @@ -100,8 +130,9 @@ private: bool animate_physical_bones = true; Vector<Bone> bones; - Vector<int> process_order; - bool process_order_dirty = true; + bool process_order_dirty; + + Vector<int> parentless_bones; void _make_dirty(); bool dirty = false; @@ -117,7 +148,20 @@ protected: void _notification(int p_what); static void _bind_methods(); +#ifndef _3D_DISABLED + Ref<SkeletonModificationStack3D> modification_stack; +#endif // _3D_DISABLED + public: + enum Bone_Forward_Axis { + BONE_AXIS_X_FORWARD = 0, + BONE_AXIS_Y_FORWARD = 1, + BONE_AXIS_Z_FORWARD = 2, + BONE_AXIS_NEGATIVE_X_FORWARD = 3, + BONE_AXIS_NEGATIVE_Y_FORWARD = 4, + BONE_AXIS_NEGATIVE_Z_FORWARD = 5, + }; + enum { NOTIFICATION_UPDATE_SKELETON = 50 }; @@ -135,6 +179,12 @@ public: void unparent_bone_and_rest(int p_bone); + Vector<int> get_bone_children(int p_bone); + void set_bone_children(int p_bone, Vector<int> p_children); + void add_bone_child(int p_bone, int p_child); + void remove_bone_child(int p_bone, int p_child); + Vector<int> get_parentless_bones(); + void set_bone_disable_rest(int p_bone, bool p_disable); bool is_bone_rest_disabled(int p_bone) const; @@ -145,16 +195,8 @@ public: Transform3D get_bone_global_pose(int p_bone) const; Transform3D get_bone_global_pose_no_override(int p_bone) const; - void clear_bones_global_pose_override(); - void set_bone_global_pose_override(int p_bone, const Transform3D &p_pose, real_t p_amount, bool p_persistent = false); - void set_bone_enabled(int p_bone, bool p_enabled); bool is_bone_enabled(int p_bone) const; - - void bind_child_node_to_bone(int p_bone, Node *p_node); - void unbind_child_node_from_bone(int p_bone, Node *p_node); - void get_bound_child_nodes_to_bone(int p_bone, List<Node *> *p_bound) const; - void clear_bones(); // posing api @@ -165,15 +207,40 @@ public: void set_bone_custom_pose(int p_bone, const Transform3D &p_custom_pose); Transform3D get_bone_custom_pose(int p_bone) const; + void clear_bones_global_pose_override(); + Transform3D get_bone_global_pose_override(int p_bone) const; + void set_bone_global_pose_override(int p_bone, const Transform3D &p_pose, real_t p_amount, bool p_persistent = false); + + void clear_bones_local_pose_override(); + Transform3D get_bone_local_pose_override(int p_bone) const; + void set_bone_local_pose_override(int p_bone, const Transform3D &p_pose, real_t p_amount, bool p_persistent = false); + void localize_rests(); // used for loaders and tools - int get_process_order(int p_idx); - Vector<int> get_bone_process_orders(); Ref<SkinReference> register_skin(const Ref<Skin> &p_skin); + void force_update_all_bone_transforms(); + void force_update_bone_children_transforms(int bone_idx); + + void update_bone_rest_forward_vector(int p_bone, bool p_force_update = false); + void update_bone_rest_forward_axis(int p_bone, bool p_force_update = false); + Vector3 get_bone_axis_forward_vector(int p_bone); + int get_bone_axis_forward_enum(int p_bone); + // Helper functions - Transform3D bone_transform_to_world_transform(Transform3D p_transform); - Transform3D world_transform_to_bone_transform(Transform3D p_transform); + Transform3D global_pose_to_world_transform(Transform3D p_global_pose); + Transform3D world_transform_to_global_pose(Transform3D p_transform); + Transform3D global_pose_to_local_pose(int p_bone_idx, Transform3D p_global_pose); + Transform3D local_pose_to_global_pose(int p_bone_idx, Transform3D p_local_pose); + + Basis global_pose_z_forward_to_bone_forward(int p_bone_idx, Basis p_basis); + + // Modifications +#ifndef _3D_DISABLED + Ref<SkeletonModificationStack3D> get_modification_stack(); + void set_modification_stack(Ref<SkeletonModificationStack3D> p_stack); + void execute_modifications(real_t p_delta, int p_execution_mode); +#endif // _3D_DISABLED // Physical bone API diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index 47c7e57e3f..40ab439a51 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -168,7 +168,15 @@ #include "scene/resources/skeleton_modification_2d_physicalbones.h" #include "scene/resources/skeleton_modification_2d_stackholder.h" #include "scene/resources/skeleton_modification_2d_twoboneik.h" +#include "scene/resources/skeleton_modification_3d.h" +#include "scene/resources/skeleton_modification_3d_ccdik.h" +#include "scene/resources/skeleton_modification_3d_fabrik.h" +#include "scene/resources/skeleton_modification_3d_jiggle.h" +#include "scene/resources/skeleton_modification_3d_lookat.h" +#include "scene/resources/skeleton_modification_3d_stackholder.h" +#include "scene/resources/skeleton_modification_3d_twoboneik.h" #include "scene/resources/skeleton_modification_stack_2d.h" +#include "scene/resources/skeleton_modification_stack_3d.h" #include "scene/resources/sky.h" #include "scene/resources/sky_material.h" #include "scene/resources/sphere_shape_3d.h" @@ -754,6 +762,15 @@ void register_scene_types() { GDREGISTER_CLASS(ConvexPolygonShape3D); GDREGISTER_CLASS(ConcavePolygonShape3D); + ClassDB::register_class<SkeletonModificationStack3D>(); + ClassDB::register_class<SkeletonModification3D>(); + ClassDB::register_class<SkeletonModification3DLookAt>(); + ClassDB::register_class<SkeletonModification3DCCDIK>(); + ClassDB::register_class<SkeletonModification3DFABRIK>(); + ClassDB::register_class<SkeletonModification3DJiggle>(); + ClassDB::register_class<SkeletonModification3DTwoBoneIK>(); + ClassDB::register_class<SkeletonModification3DStackHolder>(); + OS::get_singleton()->yield(); //may take time to init GDREGISTER_CLASS(VelocityTracker3D); diff --git a/scene/resources/skeleton_modification_3d.cpp b/scene/resources/skeleton_modification_3d.cpp new file mode 100644 index 0000000000..9306ee14cd --- /dev/null +++ b/scene/resources/skeleton_modification_3d.cpp @@ -0,0 +1,170 @@ +/*************************************************************************/ +/* skeleton_modification_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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_modification_3d.h" +#include "scene/3d/skeleton_3d.h" + +void SkeletonModification3D::_execute(real_t p_delta) { + if (get_script_instance()) { + if (get_script_instance()->has_method("execute")) { + get_script_instance()->call("execute", p_delta); + } + } + + if (!enabled) + return; +} + +void SkeletonModification3D::_setup_modification(SkeletonModificationStack3D *p_stack) { + stack = p_stack; + if (stack) { + is_setup = true; + } else { + WARN_PRINT("Could not setup modification with name " + this->get_name()); + } + + if (get_script_instance()) { + if (get_script_instance()->has_method("setup_modification")) { + get_script_instance()->call("setup_modification", p_stack); + } + } +} + +void SkeletonModification3D::set_enabled(bool p_enabled) { + enabled = p_enabled; +} + +bool SkeletonModification3D::get_enabled() { + return enabled; +} + +// Helper function. Needed for CCDIK. +real_t SkeletonModification3D::clamp_angle(real_t p_angle, real_t p_min_bound, real_t p_max_bound, bool p_invert) { + // Map to the 0 to 360 range (in radians though) instead of the -180 to 180 range. + if (p_angle < 0) { + p_angle = Math_TAU + p_angle; + } + + // Make min and max in the range of 0 to 360 (in radians), and make sure they are in the right order + if (p_min_bound < 0) { + p_min_bound = Math_TAU + p_min_bound; + } + if (p_max_bound < 0) { + p_max_bound = Math_TAU + p_max_bound; + } + if (p_min_bound > p_max_bound) { + real_t tmp = p_min_bound; + p_min_bound = p_max_bound; + p_max_bound = tmp; + } + + // Note: May not be the most optimal way to clamp, but it always constraints to the nearest angle. + if (p_invert == false) { + if (p_angle < p_min_bound || p_angle > p_max_bound) { + Vector2 min_bound_vec = Vector2(Math::cos(p_min_bound), Math::sin(p_min_bound)); + Vector2 max_bound_vec = Vector2(Math::cos(p_max_bound), Math::sin(p_max_bound)); + Vector2 angle_vec = Vector2(Math::cos(p_angle), Math::sin(p_angle)); + + if (angle_vec.distance_squared_to(min_bound_vec) <= angle_vec.distance_squared_to(max_bound_vec)) { + p_angle = p_min_bound; + } else { + p_angle = p_max_bound; + } + } + } else { + if (p_angle > p_min_bound && p_angle < p_max_bound) { + Vector2 min_bound_vec = Vector2(Math::cos(p_min_bound), Math::sin(p_min_bound)); + Vector2 max_bound_vec = Vector2(Math::cos(p_max_bound), Math::sin(p_max_bound)); + Vector2 angle_vec = Vector2(Math::cos(p_angle), Math::sin(p_angle)); + + if (angle_vec.distance_squared_to(min_bound_vec) <= angle_vec.distance_squared_to(max_bound_vec)) { + p_angle = p_min_bound; + } else { + p_angle = p_max_bound; + } + } + } + return p_angle; +} + +bool SkeletonModification3D::_print_execution_error(bool p_condition, String p_message) { + // If the modification is not setup, don't bother printing the error + if (!is_setup) { + return p_condition; + } + + if (p_condition && !execution_error_found) { + ERR_PRINT(p_message); + execution_error_found = true; + } + return p_condition; +} + +Ref<SkeletonModificationStack3D> SkeletonModification3D::get_modification_stack() { + return stack; +} + +void SkeletonModification3D::set_is_setup(bool p_is_setup) { + is_setup = p_is_setup; +} + +bool SkeletonModification3D::get_is_setup() const { + return is_setup; +} + +void SkeletonModification3D::set_execution_mode(int p_mode) { + execution_mode = p_mode; +} + +int SkeletonModification3D::get_execution_mode() const { + return execution_mode; +} + +void SkeletonModification3D::_bind_methods() { + BIND_VMETHOD(MethodInfo("_execute", PropertyInfo(Variant::FLOAT, "delta"))); + BIND_VMETHOD(MethodInfo("_setup_modification", PropertyInfo(Variant::OBJECT, "modification_stack", PROPERTY_HINT_RESOURCE_TYPE, "SkeletonModificationStack3D"))); + + ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &SkeletonModification3D::set_enabled); + ClassDB::bind_method(D_METHOD("get_enabled"), &SkeletonModification3D::get_enabled); + ClassDB::bind_method(D_METHOD("get_modification_stack"), &SkeletonModification3D::get_modification_stack); + ClassDB::bind_method(D_METHOD("set_is_setup", "is_setup"), &SkeletonModification3D::set_is_setup); + ClassDB::bind_method(D_METHOD("get_is_setup"), &SkeletonModification3D::get_is_setup); + ClassDB::bind_method(D_METHOD("set_execution_mode", "execution_mode"), &SkeletonModification3D::set_execution_mode); + ClassDB::bind_method(D_METHOD("get_execution_mode"), &SkeletonModification3D::get_execution_mode); + ClassDB::bind_method(D_METHOD("clamp_angle", "angle", "min", "max", "invert"), &SkeletonModification3D::clamp_angle); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "get_enabled"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "execution_mode", PROPERTY_HINT_ENUM, "process, physics_process"), "set_execution_mode", "get_execution_mode"); +} + +SkeletonModification3D::SkeletonModification3D() { + stack = nullptr; + is_setup = false; +} diff --git a/scene/resources/skeleton_modification_3d.h b/scene/resources/skeleton_modification_3d.h new file mode 100644 index 0000000000..94ab0bf32c --- /dev/null +++ b/scene/resources/skeleton_modification_3d.h @@ -0,0 +1,76 @@ +/*************************************************************************/ +/* skeleton_modification_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 SKELETONMODIFICATION3D_H +#define SKELETONMODIFICATION3D_H + +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_stack_3d.h" + +class SkeletonModificationStack3D; + +class SkeletonModification3D : public Resource { + GDCLASS(SkeletonModification3D, Resource); + friend class Skeleton3D; + friend class SkeletonModificationStack3D; + +protected: + static void _bind_methods(); + + SkeletonModificationStack3D *stack; + int execution_mode = 0; // 0 = process + + bool enabled = true; + bool is_setup = false; + bool execution_error_found = false; + + bool _print_execution_error(bool p_condition, String p_message); + +public: + virtual void _execute(real_t p_delta); + virtual void _setup_modification(SkeletonModificationStack3D *p_stack); + + real_t clamp_angle(real_t p_angle, real_t p_min_bound, real_t p_max_bound, bool p_invert); + + void set_enabled(bool p_enabled); + bool get_enabled(); + + void set_execution_mode(int p_mode); + int get_execution_mode() const; + + Ref<SkeletonModificationStack3D> get_modification_stack(); + + void set_is_setup(bool p_setup); + bool get_is_setup() const; + + SkeletonModification3D(); +}; + +#endif // SKELETONMODIFICATION3D_H diff --git a/scene/resources/skeleton_modification_3d_ccdik.cpp b/scene/resources/skeleton_modification_3d_ccdik.cpp new file mode 100644 index 0000000000..6409022563 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_ccdik.cpp @@ -0,0 +1,474 @@ +/*************************************************************************/ +/* skeleton_modification_3d_ccdik.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/resources/skeleton_modification_3d_ccdik.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +bool SkeletonModification3DCCDIK::_set(const StringName &p_path, const Variant &p_value) { + String path = p_path; + + if (path.begins_with("joint_data/")) { + int ccdik_data_size = ccdik_data_chain.size(); + int which = path.get_slicec('/', 1).to_int(); + String what = path.get_slicec('/', 2); + ERR_FAIL_INDEX_V(which, ccdik_data_size, false); + + if (what == "bone_name") { + set_ccdik_joint_bone_name(which, p_value); + } else if (what == "bone_index") { + set_ccdik_joint_bone_index(which, p_value); + } else if (what == "ccdik_axis") { + set_ccdik_joint_ccdik_axis(which, p_value); + } else if (what == "enable_joint_constraint") { + set_ccdik_joint_enable_constraint(which, p_value); + } else if (what == "joint_constraint_angle_min") { + set_ccdik_joint_constraint_angle_min(which, Math::deg2rad(real_t(p_value))); + } else if (what == "joint_constraint_angle_max") { + set_ccdik_joint_constraint_angle_max(which, Math::deg2rad(real_t(p_value))); + } else if (what == "joint_constraint_angles_invert") { + set_ccdik_joint_constraint_invert(which, p_value); + } + return true; + } + return true; +} + +bool SkeletonModification3DCCDIK::_get(const StringName &p_path, Variant &r_ret) const { + String path = p_path; + + if (path.begins_with("joint_data/")) { + const int ccdik_data_size = ccdik_data_chain.size(); + int which = path.get_slicec('/', 1).to_int(); + String what = path.get_slicec('/', 2); + ERR_FAIL_INDEX_V(which, ccdik_data_size, false); + + if (what == "bone_name") { + r_ret = get_ccdik_joint_bone_name(which); + } else if (what == "bone_index") { + r_ret = get_ccdik_joint_bone_index(which); + } else if (what == "ccdik_axis") { + r_ret = get_ccdik_joint_ccdik_axis(which); + } else if (what == "enable_joint_constraint") { + r_ret = get_ccdik_joint_enable_constraint(which); + } else if (what == "joint_constraint_angle_min") { + r_ret = Math::rad2deg(get_ccdik_joint_constraint_angle_min(which)); + } else if (what == "joint_constraint_angle_max") { + r_ret = Math::rad2deg(get_ccdik_joint_constraint_angle_max(which)); + } else if (what == "joint_constraint_angles_invert") { + r_ret = get_ccdik_joint_constraint_invert(which); + } + return true; + } + return true; +} + +void SkeletonModification3DCCDIK::_get_property_list(List<PropertyInfo> *p_list) const { + for (uint32_t i = 0; i < ccdik_data_chain.size(); i++) { + String base_string = "joint_data/" + itos(i) + "/"; + + p_list->push_back(PropertyInfo(Variant::STRING_NAME, base_string + "bone_name", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::INT, base_string + "bone_index", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + + p_list->push_back(PropertyInfo(Variant::INT, base_string + "ccdik_axis", + PROPERTY_HINT_ENUM, "X Axis, Y Axis, Z Axis", PROPERTY_USAGE_DEFAULT)); + + p_list->push_back(PropertyInfo(Variant::BOOL, base_string + "enable_joint_constraint", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + if (ccdik_data_chain[i].enable_constraint) { + p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "joint_constraint_angle_min", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "joint_constraint_angle_max", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::BOOL, base_string + "joint_constraint_angles_invert", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + } + } +} + +void SkeletonModification3DCCDIK::_execute(real_t p_delta) { + ERR_FAIL_COND_MSG(!stack || !is_setup || stack->skeleton == nullptr, + "Modification is not setup and therefore cannot execute!"); + if (!enabled) { + return; + } + + if (target_node_cache.is_null()) { + _print_execution_error(true, "Target cache is out of date. Attempting to update"); + update_target_cache(); + return; + } + if (tip_node_cache.is_null()) { + _print_execution_error(true, "Tip cache is out of date. Attempting to update"); + update_tip_cache(); + return; + } + + // Reset the local bone overrides for CCDIK affected nodes + for (uint32_t i = 0; i < ccdik_data_chain.size(); i++) { + stack->skeleton->set_bone_local_pose_override(ccdik_data_chain[i].bone_idx, + stack->skeleton->get_bone_local_pose_override(ccdik_data_chain[i].bone_idx), + 0.0, false); + } + + Node3D *node_target = Object::cast_to<Node3D>(ObjectDB::get_instance(target_node_cache)); + Node3D *node_tip = Object::cast_to<Node3D>(ObjectDB::get_instance(tip_node_cache)); + + if (_print_execution_error(!node_target || !node_target->is_inside_tree(), "Target node is not in the scene tree. Cannot execute modification!")) { + return; + } + if (_print_execution_error(!node_tip || !node_tip->is_inside_tree(), "Tip node is not in the scene tree. Cannot execute modification!")) { + return; + } + + if (use_high_quality_solve) { + for (uint32_t i = 0; i < ccdik_data_chain.size(); i++) { + for (uint32_t j = i; j < ccdik_data_chain.size(); j++) { + _execute_ccdik_joint(j, node_target, node_tip); + } + } + } else { + for (uint32_t i = 0; i < ccdik_data_chain.size(); i++) { + _execute_ccdik_joint(i, node_target, node_tip); + } + } + + execution_error_found = false; +} + +void SkeletonModification3DCCDIK::_execute_ccdik_joint(int p_joint_idx, Node3D *p_target, Node3D *p_tip) { + CCDIK_Joint_Data ccdik_data = ccdik_data_chain[p_joint_idx]; + + if (_print_execution_error(ccdik_data.bone_idx < 0 || ccdik_data.bone_idx > stack->skeleton->get_bone_count(), + "CCDIK joint: bone index for joint" + itos(p_joint_idx) + " not found. Cannot execute modification!")) { + return; + } + + Transform3D bone_trans = stack->skeleton->global_pose_to_local_pose(ccdik_data.bone_idx, stack->skeleton->get_bone_global_pose(ccdik_data.bone_idx)); + Transform3D tip_trans = stack->skeleton->global_pose_to_local_pose(ccdik_data.bone_idx, stack->skeleton->world_transform_to_global_pose(p_tip->get_global_transform())); + Transform3D target_trans = stack->skeleton->global_pose_to_local_pose(ccdik_data.bone_idx, stack->skeleton->world_transform_to_global_pose(p_target->get_global_transform())); + + if (tip_trans.origin.distance_to(target_trans.origin) <= 0.01) { + return; + } + + // Inspired (and very loosely based on) by the CCDIK algorithm made by Zalo on GitHub (https://github.com/zalo/MathUtilities) + // Convert the 3D position to a 2D position so we can use Atan2 (via the angle function) + // to know how much rotation we need on the given axis to place the tip at the target. + Vector2 tip_pos_2d; + Vector2 target_pos_2d; + if (ccdik_data.ccdik_axis == CCDIK_Axes::AXIS_X) { + tip_pos_2d = Vector2(tip_trans.origin.y, tip_trans.origin.z); + target_pos_2d = Vector2(target_trans.origin.y, target_trans.origin.z); + bone_trans.basis.rotate_local(Vector3(1, 0, 0), target_pos_2d.angle() - tip_pos_2d.angle()); + } else if (ccdik_data.ccdik_axis == CCDIK_Axes::AXIS_Y) { + tip_pos_2d = Vector2(tip_trans.origin.z, tip_trans.origin.x); + target_pos_2d = Vector2(target_trans.origin.z, target_trans.origin.x); + bone_trans.basis.rotate_local(Vector3(0, 1, 0), target_pos_2d.angle() - tip_pos_2d.angle()); + } else if (ccdik_data.ccdik_axis == CCDIK_Axes::AXIS_Z) { + tip_pos_2d = Vector2(tip_trans.origin.x, tip_trans.origin.y); + target_pos_2d = Vector2(target_trans.origin.x, target_trans.origin.y); + bone_trans.basis.rotate_local(Vector3(0, 0, 1), target_pos_2d.angle() - tip_pos_2d.angle()); + } else { + // Should never happen, but... + ERR_FAIL_MSG("CCDIK joint: Unknown axis vector passed for joint" + itos(p_joint_idx) + ". Cannot execute modification!"); + } + + if (ccdik_data.enable_constraint) { + Vector3 rotation_axis; + real_t rotation_angle; + bone_trans.basis.get_axis_angle(rotation_axis, rotation_angle); + + // Note: When the axis has a negative direction, the angle is OVER 180 degrees and therefore we need to account for this + // when constraining. + if (ccdik_data.ccdik_axis == CCDIK_Axes::AXIS_X) { + if (rotation_axis.x < 0) { + rotation_angle += Math_PI; + rotation_axis = Vector3(1, 0, 0); + } + } else if (ccdik_data.ccdik_axis == CCDIK_Axes::AXIS_Y) { + if (rotation_axis.y < 0) { + rotation_angle += Math_PI; + rotation_axis = Vector3(0, 1, 0); + } + } else if (ccdik_data.ccdik_axis == CCDIK_Axes::AXIS_Z) { + if (rotation_axis.z < 0) { + rotation_angle += Math_PI; + rotation_axis = Vector3(0, 0, 1); + } + } else { + // Should never happen, but... + ERR_FAIL_MSG("CCDIK joint: Unknown axis vector passed for joint" + itos(p_joint_idx) + ". Cannot execute modification!"); + } + rotation_angle = clamp_angle(rotation_angle, ccdik_data.constraint_angle_min, ccdik_data.constraint_angle_max, ccdik_data.constraint_angles_invert); + + bone_trans.basis.set_axis_angle(rotation_axis, rotation_angle); + } + + stack->skeleton->set_bone_local_pose_override(ccdik_data.bone_idx, bone_trans, stack->strength, true); + stack->skeleton->force_update_bone_children_transforms(ccdik_data.bone_idx); +} + +void SkeletonModification3DCCDIK::_setup_modification(SkeletonModificationStack3D *p_stack) { + stack = p_stack; + if (stack != nullptr) { + is_setup = true; + execution_error_found = false; + update_target_cache(); + update_tip_cache(); + } +} + +void SkeletonModification3DCCDIK::update_target_cache() { + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update target cache: modification is not properly setup!"); + return; + } + + target_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree()) { + if (stack->skeleton->has_node(target_node)) { + Node *node = stack->skeleton->get_node(target_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update target cache: node is this modification's skeleton or cannot be found!"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update target cache: node is not in scene tree!"); + target_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DCCDIK::update_tip_cache() { + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update tip cache: modification is not properly setup!"); + return; + } + + tip_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree()) { + if (stack->skeleton->has_node(tip_node)) { + Node *node = stack->skeleton->get_node(tip_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update tip cache: node is this modification's skeleton or cannot be found!"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update tip cache: node is not in scene tree!"); + tip_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DCCDIK::set_target_node(const NodePath &p_target_node) { + target_node = p_target_node; + update_target_cache(); +} + +NodePath SkeletonModification3DCCDIK::get_target_node() const { + return target_node; +} + +void SkeletonModification3DCCDIK::set_tip_node(const NodePath &p_tip_node) { + tip_node = p_tip_node; + update_tip_cache(); +} + +NodePath SkeletonModification3DCCDIK::get_tip_node() const { + return tip_node; +} + +void SkeletonModification3DCCDIK::set_use_high_quality_solve(bool p_high_quality) { + use_high_quality_solve = p_high_quality; +} + +bool SkeletonModification3DCCDIK::get_use_high_quality_solve() const { + return use_high_quality_solve; +} + +// CCDIK joint data functions +String SkeletonModification3DCCDIK::get_ccdik_joint_bone_name(int p_joint_idx) const { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, String()); + return ccdik_data_chain[p_joint_idx].bone_name; +} + +void SkeletonModification3DCCDIK::set_ccdik_joint_bone_name(int p_joint_idx, String p_bone_name) { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ccdik_data_chain[p_joint_idx].bone_name = p_bone_name; + + if (stack) { + if (stack->skeleton) { + ccdik_data_chain[p_joint_idx].bone_idx = stack->skeleton->find_bone(p_bone_name); + } + } + execution_error_found = false; + notify_property_list_changed(); +} + +int SkeletonModification3DCCDIK::get_ccdik_joint_bone_index(int p_joint_idx) const { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, -1); + return ccdik_data_chain[p_joint_idx].bone_idx; +} + +void SkeletonModification3DCCDIK::set_ccdik_joint_bone_index(int p_joint_idx, int p_bone_idx) { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ERR_FAIL_COND_MSG(p_bone_idx < 0, "Bone index is out of range: The index is too low!"); + ccdik_data_chain[p_joint_idx].bone_idx = p_bone_idx; + + if (stack) { + if (stack->skeleton) { + ccdik_data_chain[p_joint_idx].bone_name = stack->skeleton->get_bone_name(p_bone_idx); + } + } + execution_error_found = false; + notify_property_list_changed(); +} + +int SkeletonModification3DCCDIK::get_ccdik_joint_ccdik_axis(int p_joint_idx) const { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, -1); + return ccdik_data_chain[p_joint_idx].ccdik_axis; +} + +void SkeletonModification3DCCDIK::set_ccdik_joint_ccdik_axis(int p_joint_idx, int p_axis) { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ERR_FAIL_COND_MSG(p_axis < 0, "CCDIK axis is out of range: The axis mode is too low!"); + ccdik_data_chain[p_joint_idx].ccdik_axis = p_axis; + notify_property_list_changed(); +} + +bool SkeletonModification3DCCDIK::get_ccdik_joint_enable_constraint(int p_joint_idx) const { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return ccdik_data_chain[p_joint_idx].enable_constraint; +} + +void SkeletonModification3DCCDIK::set_ccdik_joint_enable_constraint(int p_joint_idx, bool p_enable) { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ccdik_data_chain[p_joint_idx].enable_constraint = p_enable; + notify_property_list_changed(); +} + +real_t SkeletonModification3DCCDIK::get_ccdik_joint_constraint_angle_min(int p_joint_idx) const { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return ccdik_data_chain[p_joint_idx].constraint_angle_min; +} + +void SkeletonModification3DCCDIK::set_ccdik_joint_constraint_angle_min(int p_joint_idx, real_t p_angle_min) { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ccdik_data_chain[p_joint_idx].constraint_angle_min = p_angle_min; +} + +real_t SkeletonModification3DCCDIK::get_ccdik_joint_constraint_angle_max(int p_joint_idx) const { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return ccdik_data_chain[p_joint_idx].constraint_angle_max; +} + +void SkeletonModification3DCCDIK::set_ccdik_joint_constraint_angle_max(int p_joint_idx, real_t p_angle_max) { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ccdik_data_chain[p_joint_idx].constraint_angle_max = p_angle_max; +} + +bool SkeletonModification3DCCDIK::get_ccdik_joint_constraint_invert(int p_joint_idx) const { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return ccdik_data_chain[p_joint_idx].constraint_angles_invert; +} + +void SkeletonModification3DCCDIK::set_ccdik_joint_constraint_invert(int p_joint_idx, bool p_invert) { + const int bone_chain_size = ccdik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ccdik_data_chain[p_joint_idx].constraint_angles_invert = p_invert; +} + +int SkeletonModification3DCCDIK::get_ccdik_data_chain_length() { + return ccdik_data_chain.size(); +} +void SkeletonModification3DCCDIK::set_ccdik_data_chain_length(int p_length) { + ERR_FAIL_COND(p_length < 0); + ccdik_data_chain.resize(p_length); + execution_error_found = false; + notify_property_list_changed(); +} + +void SkeletonModification3DCCDIK::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_target_node", "target_nodepath"), &SkeletonModification3DCCDIK::set_target_node); + ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonModification3DCCDIK::get_target_node); + + ClassDB::bind_method(D_METHOD("set_tip_node", "tip_nodepath"), &SkeletonModification3DCCDIK::set_tip_node); + ClassDB::bind_method(D_METHOD("get_tip_node"), &SkeletonModification3DCCDIK::get_tip_node); + + ClassDB::bind_method(D_METHOD("set_use_high_quality_solve", "high_quality_solve"), &SkeletonModification3DCCDIK::set_use_high_quality_solve); + ClassDB::bind_method(D_METHOD("get_use_high_quality_solve"), &SkeletonModification3DCCDIK::get_use_high_quality_solve); + + // CCDIK joint data functions + ClassDB::bind_method(D_METHOD("get_ccdik_joint_bone_name", "joint_idx"), &SkeletonModification3DCCDIK::get_ccdik_joint_bone_name); + ClassDB::bind_method(D_METHOD("set_ccdik_joint_bone_name", "joint_idx", "bone_name"), &SkeletonModification3DCCDIK::set_ccdik_joint_bone_name); + ClassDB::bind_method(D_METHOD("get_ccdik_joint_bone_index", "joint_idx"), &SkeletonModification3DCCDIK::get_ccdik_joint_bone_index); + ClassDB::bind_method(D_METHOD("set_ccdik_joint_bone_index", "joint_idx", "bone_index"), &SkeletonModification3DCCDIK::set_ccdik_joint_bone_index); + ClassDB::bind_method(D_METHOD("get_ccdik_joint_ccdik_axis", "joint_idx"), &SkeletonModification3DCCDIK::get_ccdik_joint_ccdik_axis); + ClassDB::bind_method(D_METHOD("set_ccdik_joint_ccdik_axis", "joint_idx", "axis"), &SkeletonModification3DCCDIK::set_ccdik_joint_ccdik_axis); + ClassDB::bind_method(D_METHOD("get_ccdik_joint_enable_joint_constraint", "joint_idx"), &SkeletonModification3DCCDIK::get_ccdik_joint_enable_constraint); + ClassDB::bind_method(D_METHOD("set_ccdik_joint_enable_joint_constraint", "joint_idx", "enable"), &SkeletonModification3DCCDIK::set_ccdik_joint_enable_constraint); + ClassDB::bind_method(D_METHOD("get_ccdik_joint_constraint_angle_min", "joint_idx"), &SkeletonModification3DCCDIK::get_ccdik_joint_constraint_angle_min); + ClassDB::bind_method(D_METHOD("set_ccdik_joint_constraint_angle_min", "joint_idx", "min_angle"), &SkeletonModification3DCCDIK::set_ccdik_joint_constraint_angle_min); + ClassDB::bind_method(D_METHOD("get_ccdik_joint_constraint_angle_max", "joint_idx"), &SkeletonModification3DCCDIK::get_ccdik_joint_constraint_angle_max); + ClassDB::bind_method(D_METHOD("set_ccdik_joint_constraint_angle_max", "joint_idx", "max_angle"), &SkeletonModification3DCCDIK::set_ccdik_joint_constraint_angle_max); + ClassDB::bind_method(D_METHOD("get_ccdik_joint_constraint_invert", "joint_idx"), &SkeletonModification3DCCDIK::get_ccdik_joint_constraint_invert); + ClassDB::bind_method(D_METHOD("set_ccdik_joint_constraint_invert", "joint_idx", "invert"), &SkeletonModification3DCCDIK::set_ccdik_joint_constraint_invert); + + ClassDB::bind_method(D_METHOD("set_ccdik_data_chain_length", "length"), &SkeletonModification3DCCDIK::set_ccdik_data_chain_length); + ClassDB::bind_method(D_METHOD("get_ccdik_data_chain_length"), &SkeletonModification3DCCDIK::get_ccdik_data_chain_length); + + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_nodepath", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D"), "set_target_node", "get_target_node"); + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "tip_nodepath", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D"), "set_tip_node", "get_tip_node"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "high_quality_solve", PROPERTY_HINT_NONE, ""), "set_use_high_quality_solve", "get_use_high_quality_solve"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "ccdik_data_chain_length", PROPERTY_HINT_RANGE, "0,100,1"), "set_ccdik_data_chain_length", "get_ccdik_data_chain_length"); +} + +SkeletonModification3DCCDIK::SkeletonModification3DCCDIK() { + stack = nullptr; + is_setup = false; + enabled = true; +} + +SkeletonModification3DCCDIK::~SkeletonModification3DCCDIK() { +} diff --git a/scene/resources/skeleton_modification_3d_ccdik.h b/scene/resources/skeleton_modification_3d_ccdik.h new file mode 100644 index 0000000000..e7537cc5b0 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_ccdik.h @@ -0,0 +1,114 @@ +/*************************************************************************/ +/* skeleton_modification_3d_ccdik.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "core/templates/local_vector.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +#ifndef SKELETONMODIFICATION3DCCDIK_H +#define SKELETONMODIFICATION3DCCDIK_H + +class SkeletonModification3DCCDIK : public SkeletonModification3D { + GDCLASS(SkeletonModification3DCCDIK, SkeletonModification3D); + +private: + enum CCDIK_Axes { + AXIS_X, + AXIS_Y, + AXIS_Z + }; + + struct CCDIK_Joint_Data { + String bone_name = ""; + int bone_idx = -1; + int ccdik_axis = 0; + + bool enable_constraint = false; + real_t constraint_angle_min = 0; + real_t constraint_angle_max = (2.0 * Math_PI); + bool constraint_angles_invert = false; + }; + + LocalVector<CCDIK_Joint_Data> ccdik_data_chain; + NodePath target_node; + ObjectID target_node_cache; + + NodePath tip_node; + ObjectID tip_node_cache; + + bool use_high_quality_solve = true; + + void update_target_cache(); + void update_tip_cache(); + + void _execute_ccdik_joint(int p_joint_idx, Node3D *p_target, Node3D *p_tip); + +protected: + static void _bind_methods(); + bool _get(const StringName &p_path, Variant &r_ret) const; + bool _set(const StringName &p_path, const Variant &p_value); + void _get_property_list(List<PropertyInfo> *p_list) const; + +public: + virtual void _execute(real_t p_delta) override; + virtual void _setup_modification(SkeletonModificationStack3D *p_stack) override; + + void set_target_node(const NodePath &p_target_node); + NodePath get_target_node() const; + + void set_tip_node(const NodePath &p_tip_node); + NodePath get_tip_node() const; + + void set_use_high_quality_solve(bool p_solve); + bool get_use_high_quality_solve() const; + + String get_ccdik_joint_bone_name(int p_joint_idx) const; + void set_ccdik_joint_bone_name(int p_joint_idx, String p_bone_name); + int get_ccdik_joint_bone_index(int p_joint_idx) const; + void set_ccdik_joint_bone_index(int p_joint_idx, int p_bone_idx); + int get_ccdik_joint_ccdik_axis(int p_joint_idx) const; + void set_ccdik_joint_ccdik_axis(int p_joint_idx, int p_axis); + bool get_ccdik_joint_enable_constraint(int p_joint_idx) const; + void set_ccdik_joint_enable_constraint(int p_joint_idx, bool p_enable); + real_t get_ccdik_joint_constraint_angle_min(int p_joint_idx) const; + void set_ccdik_joint_constraint_angle_min(int p_joint_idx, real_t p_angle_min); + real_t get_ccdik_joint_constraint_angle_max(int p_joint_idx) const; + void set_ccdik_joint_constraint_angle_max(int p_joint_idx, real_t p_angle_max); + bool get_ccdik_joint_constraint_invert(int p_joint_idx) const; + void set_ccdik_joint_constraint_invert(int p_joint_idx, bool p_invert); + + int get_ccdik_data_chain_length(); + void set_ccdik_data_chain_length(int p_new_length); + + SkeletonModification3DCCDIK(); + ~SkeletonModification3DCCDIK(); +}; + +#endif //SKELETONMODIFICATION3DCCDIK_H diff --git a/scene/resources/skeleton_modification_3d_fabrik.cpp b/scene/resources/skeleton_modification_3d_fabrik.cpp new file mode 100644 index 0000000000..69f75eb7b5 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_fabrik.cpp @@ -0,0 +1,628 @@ +/*************************************************************************/ +/* skeleton_modification_3d_fabrik.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/resources/skeleton_modification_3d_fabrik.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +bool SkeletonModification3DFABRIK::_set(const StringName &p_path, const Variant &p_value) { + String path = p_path; + + if (path.begins_with("joint_data/")) { + int fabrik_data_size = fabrik_data_chain.size(); + int which = path.get_slicec('/', 1).to_int(); + String what = path.get_slicec('/', 2); + ERR_FAIL_INDEX_V(which, fabrik_data_size, false); + + if (what == "bone_name") { + set_fabrik_joint_bone_name(which, p_value); + } else if (what == "bone_index") { + set_fabrik_joint_bone_index(which, p_value); + } else if (what == "length") { + set_fabrik_joint_length(which, p_value); + } else if (what == "magnet_position") { + set_fabrik_joint_magnet(which, p_value); + } else if (what == "auto_calculate_length") { + set_fabrik_joint_auto_calculate_length(which, p_value); + } else if (what == "use_tip_node") { + set_fabrik_joint_use_tip_node(which, p_value); + } else if (what == "tip_node") { + set_fabrik_joint_tip_node(which, p_value); + } else if (what == "use_target_basis") { + set_fabrik_joint_use_target_basis(which, p_value); + } else if (what == "roll") { + set_fabrik_joint_roll(which, Math::deg2rad(real_t(p_value))); + } + return true; + } + return true; +} + +bool SkeletonModification3DFABRIK::_get(const StringName &p_path, Variant &r_ret) const { + String path = p_path; + + if (path.begins_with("joint_data/")) { + const int fabrik_data_size = fabrik_data_chain.size(); + int which = path.get_slicec('/', 1).to_int(); + String what = path.get_slicec('/', 2); + ERR_FAIL_INDEX_V(which, fabrik_data_size, false); + + if (what == "bone_name") { + r_ret = get_fabrik_joint_bone_name(which); + } else if (what == "bone_index") { + r_ret = get_fabrik_joint_bone_index(which); + } else if (what == "length") { + r_ret = get_fabrik_joint_length(which); + } else if (what == "magnet_position") { + r_ret = get_fabrik_joint_magnet(which); + } else if (what == "auto_calculate_length") { + r_ret = get_fabrik_joint_auto_calculate_length(which); + } else if (what == "use_tip_node") { + r_ret = get_fabrik_joint_use_tip_node(which); + } else if (what == "tip_node") { + r_ret = get_fabrik_joint_tip_node(which); + } else if (what == "use_target_basis") { + r_ret = get_fabrik_joint_use_target_basis(which); + } else if (what == "roll") { + r_ret = Math::rad2deg(get_fabrik_joint_roll(which)); + } + return true; + } + return true; +} + +void SkeletonModification3DFABRIK::_get_property_list(List<PropertyInfo> *p_list) const { + for (uint32_t i = 0; i < fabrik_data_chain.size(); i++) { + String base_string = "joint_data/" + itos(i) + "/"; + + p_list->push_back(PropertyInfo(Variant::STRING_NAME, base_string + "bone_name", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::INT, base_string + "bone_index", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "roll", PROPERTY_HINT_RANGE, "-360,360,0.01", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::BOOL, base_string + "auto_calculate_length", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + + if (!fabrik_data_chain[i].auto_calculate_length) { + p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "length", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + } else { + p_list->push_back(PropertyInfo(Variant::BOOL, base_string + "use_tip_node", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + if (fabrik_data_chain[i].use_tip_node) { + p_list->push_back(PropertyInfo(Variant::NODE_PATH, base_string + "tip_node", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D", PROPERTY_USAGE_DEFAULT)); + } + } + + // Cannot apply magnet to the origin of the chain, as it will not do anything. + if (i > 0) { + p_list->push_back(PropertyInfo(Variant::VECTOR3, base_string + "magnet_position", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + } + // Only give the override basis option on the last bone in the chain, so only include it for the last bone. + if (i == fabrik_data_chain.size() - 1) { + p_list->push_back(PropertyInfo(Variant::BOOL, base_string + "use_target_basis", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + } + } +} + +void SkeletonModification3DFABRIK::_execute(real_t p_delta) { + ERR_FAIL_COND_MSG(!stack || !is_setup || stack->skeleton == nullptr, + "Modification is not setup and therefore cannot execute!"); + if (!enabled) { + return; + } + + if (target_node_cache.is_null()) { + _print_execution_error(true, "Target cache is out of date. Attempting to update..."); + update_target_cache(); + return; + } + + if (_print_execution_error(fabrik_data_chain.size() <= 1, "FABRIK requires at least two joints to operate. Cannot execute modification!")) { + return; + } + + Node3D *node_target = Object::cast_to<Node3D>(ObjectDB::get_instance(target_node_cache)); + if (_print_execution_error(!node_target || !node_target->is_inside_tree(), "Target node is not in the scene tree. Cannot execute modification!")) { + return; + } + + // Verify that all joints have a valid bone ID, and that all bone lengths are zero or more + // Also, while we are here, apply magnet positions. + for (uint32_t i = 0; i < fabrik_data_chain.size(); i++) { + if (_print_execution_error(fabrik_data_chain[i].bone_idx < 0, "FABRIK Joint " + itos(i) + " has an invalid bone ID. Cannot execute!")) { + return; + } + + if (fabrik_data_chain[i].length < 0 && fabrik_data_chain[i].auto_calculate_length) { + fabrik_joint_auto_calculate_length(i); + } + if (_print_execution_error(fabrik_data_chain[i].length < 0, "FABRIK Joint " + itos(i) + " has an invalid joint length. Cannot execute!")) { + return; + } + + Transform3D local_pose_override = stack->skeleton->get_bone_local_pose_override(fabrik_data_chain[i].bone_idx); + + // Apply magnet positions: + if (stack->skeleton->get_bone_parent(fabrik_data_chain[i].bone_idx) >= 0) { + int parent_bone_idx = stack->skeleton->get_bone_parent(fabrik_data_chain[i].bone_idx); + Transform3D conversion_transform = (stack->skeleton->get_bone_global_pose(parent_bone_idx) * stack->skeleton->get_bone_rest(parent_bone_idx)); + local_pose_override.origin += conversion_transform.basis.xform_inv(fabrik_data_chain[i].magnet_position); + } else { + local_pose_override.origin += fabrik_data_chain[i].magnet_position; + } + + stack->skeleton->set_bone_local_pose_override(fabrik_data_chain[i].bone_idx, local_pose_override, stack->strength, true); + } + + target_global_pose = stack->skeleton->world_transform_to_global_pose(node_target->get_global_transform()); + origin_global_pose = stack->skeleton->local_pose_to_global_pose( + fabrik_data_chain[0].bone_idx, stack->skeleton->get_bone_local_pose_override(fabrik_data_chain[0].bone_idx)); + + final_joint_idx = fabrik_data_chain.size() - 1; + real_t target_distance = stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[final_joint_idx].bone_idx, target_global_pose).origin.length(); + chain_iterations = 0; + + while (target_distance > chain_tolerance) { + chain_backwards(); + chain_forwards(); + + // update the target distance + target_distance = stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[final_joint_idx].bone_idx, target_global_pose).origin.length(); + + // update chain iterations + chain_iterations += 1; + if (chain_iterations >= chain_max_iterations) { + break; + } + } + chain_apply(); + + execution_error_found = false; +} + +void SkeletonModification3DFABRIK::chain_backwards() { + int final_bone_idx = fabrik_data_chain[final_joint_idx].bone_idx; + Transform3D final_joint_trans = stack->skeleton->local_pose_to_global_pose(final_bone_idx, stack->skeleton->get_bone_local_pose_override(final_bone_idx)); + + // Get the direction the final bone is facing in. + stack->skeleton->update_bone_rest_forward_vector(final_bone_idx); + Transform3D final_bone_direction_trans = final_joint_trans.looking_at(target_global_pose.origin, Vector3(0, 1, 0)); + final_bone_direction_trans.basis = stack->skeleton->global_pose_z_forward_to_bone_forward(final_bone_idx, final_bone_direction_trans.basis); + Vector3 direction = final_bone_direction_trans.basis.xform(stack->skeleton->get_bone_axis_forward_vector(final_bone_idx)).normalized(); + + // If set to override, then use the target's Basis rather than the bone's + if (fabrik_data_chain[final_joint_idx].use_target_basis) { + direction = target_global_pose.basis.xform(stack->skeleton->get_bone_axis_forward_vector(final_bone_idx)).normalized(); + } + + // set the position of the final joint to the target position + final_joint_trans.origin = target_global_pose.origin - (direction * fabrik_data_chain[final_joint_idx].length); + final_joint_trans = stack->skeleton->global_pose_to_local_pose(final_bone_idx, final_joint_trans); + stack->skeleton->set_bone_local_pose_override(final_bone_idx, final_joint_trans, stack->strength, true); + + // for all other joints, move them towards the target + int i = final_joint_idx; + while (i >= 1) { + int next_bone_idx = fabrik_data_chain[i].bone_idx; + Transform3D next_bone_trans = stack->skeleton->local_pose_to_global_pose(next_bone_idx, stack->skeleton->get_bone_local_pose_override(next_bone_idx)); + i -= 1; + int current_bone_idx = fabrik_data_chain[i].bone_idx; + Transform3D current_trans = stack->skeleton->local_pose_to_global_pose(current_bone_idx, stack->skeleton->get_bone_local_pose_override(current_bone_idx)); + + real_t length = fabrik_data_chain[i].length / (next_bone_trans.origin - current_trans.origin).length(); + current_trans.origin = next_bone_trans.origin.lerp(current_trans.origin, length); + + // Apply it back to the skeleton + stack->skeleton->set_bone_local_pose_override(current_bone_idx, stack->skeleton->global_pose_to_local_pose(current_bone_idx, current_trans), stack->strength, true); + } +} + +void SkeletonModification3DFABRIK::chain_forwards() { + // Set root at the initial position. + int origin_bone_idx = fabrik_data_chain[0].bone_idx; + Transform3D root_transform = stack->skeleton->local_pose_to_global_pose(origin_bone_idx, stack->skeleton->get_bone_local_pose_override(origin_bone_idx)); + root_transform.origin = origin_global_pose.origin; + stack->skeleton->set_bone_local_pose_override(origin_bone_idx, stack->skeleton->global_pose_to_local_pose(origin_bone_idx, root_transform), stack->strength, true); + + for (uint32_t i = 0; i < fabrik_data_chain.size() - 1; i++) { + int current_bone_idx = fabrik_data_chain[i].bone_idx; + Transform3D current_trans = stack->skeleton->local_pose_to_global_pose(current_bone_idx, stack->skeleton->get_bone_local_pose_override(current_bone_idx)); + int next_bone_idx = fabrik_data_chain[i + 1].bone_idx; + Transform3D next_bone_trans = stack->skeleton->local_pose_to_global_pose(next_bone_idx, stack->skeleton->get_bone_local_pose_override(next_bone_idx)); + + real_t length = fabrik_data_chain[i].length / (current_trans.origin - next_bone_trans.origin).length(); + next_bone_trans.origin = current_trans.origin.lerp(next_bone_trans.origin, length); + + // Apply it back to the skeleton + stack->skeleton->set_bone_local_pose_override(next_bone_idx, stack->skeleton->global_pose_to_local_pose(next_bone_idx, next_bone_trans), stack->strength, true); + } +} + +void SkeletonModification3DFABRIK::chain_apply() { + for (uint32_t i = 0; i < fabrik_data_chain.size(); i++) { + int current_bone_idx = fabrik_data_chain[i].bone_idx; + Transform3D current_trans = stack->skeleton->get_bone_local_pose_override(current_bone_idx); + current_trans = stack->skeleton->local_pose_to_global_pose(current_bone_idx, current_trans); + + // If this is the last bone in the chain... + if (i == fabrik_data_chain.size() - 1) { + if (fabrik_data_chain[i].use_target_basis == false) { // Point to target... + // Get the forward direction that the basis is facing in right now. + stack->skeleton->update_bone_rest_forward_vector(current_bone_idx); + Vector3 forward_vector = stack->skeleton->get_bone_axis_forward_vector(current_bone_idx); + // Rotate the bone towards the target: + current_trans.basis.rotate_to_align(forward_vector, current_trans.origin.direction_to(target_global_pose.origin)); + current_trans.basis.rotate_local(forward_vector, fabrik_data_chain[i].roll); + } else { // Use the target's Basis... + current_trans.basis = target_global_pose.basis.orthonormalized().scaled(current_trans.basis.get_scale()); + } + } else { // every other bone in the chain... + int next_bone_idx = fabrik_data_chain[i + 1].bone_idx; + Transform3D next_trans = stack->skeleton->local_pose_to_global_pose(next_bone_idx, stack->skeleton->get_bone_local_pose_override(next_bone_idx)); + + // Get the forward direction that the basis is facing in right now. + stack->skeleton->update_bone_rest_forward_vector(current_bone_idx); + Vector3 forward_vector = stack->skeleton->get_bone_axis_forward_vector(current_bone_idx); + // Rotate the bone towards the next bone in the chain: + current_trans.basis.rotate_to_align(forward_vector, current_trans.origin.direction_to(next_trans.origin)); + current_trans.basis.rotate_local(forward_vector, fabrik_data_chain[i].roll); + } + current_trans = stack->skeleton->global_pose_to_local_pose(current_bone_idx, current_trans); + current_trans.origin = Vector3(0, 0, 0); + stack->skeleton->set_bone_local_pose_override(current_bone_idx, current_trans, stack->strength, true); + } + + // Update all the bones so the next modification has up-to-date data. + stack->skeleton->force_update_all_bone_transforms(); +} + +void SkeletonModification3DFABRIK::_setup_modification(SkeletonModificationStack3D *p_stack) { + stack = p_stack; + if (stack != nullptr) { + is_setup = true; + execution_error_found = false; + update_target_cache(); + + for (uint32_t i = 0; i < fabrik_data_chain.size(); i++) { + update_joint_tip_cache(i); + } + } +} + +void SkeletonModification3DFABRIK::update_target_cache() { + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update target cache: modification is not properly setup!"); + return; + } + target_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree() && target_node.is_empty() == false) { + if (stack->skeleton->has_node(target_node)) { + Node *node = stack->skeleton->get_node(target_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update target cache: node is this modification's skeleton or cannot be found!"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update target cache: node is not in the scene tree!"); + target_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DFABRIK::update_joint_tip_cache(int p_joint_idx) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_MSG(p_joint_idx, bone_chain_size, "FABRIK joint not found"); + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update tip cache: modification is not properly setup!"); + return; + } + fabrik_data_chain[p_joint_idx].tip_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree() && fabrik_data_chain[p_joint_idx].tip_node.is_empty() == false) { + if (stack->skeleton->has_node(fabrik_data_chain[p_joint_idx].tip_node)) { + Node *node = stack->skeleton->get_node(fabrik_data_chain[p_joint_idx].tip_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update tip cache for joint " + itos(p_joint_idx) + ": node is this modification's skeleton or cannot be found!"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update tip cache for joint " + itos(p_joint_idx) + ": node is not in scene tree!"); + fabrik_data_chain[p_joint_idx].tip_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DFABRIK::set_target_node(const NodePath &p_target_node) { + target_node = p_target_node; + update_target_cache(); +} + +NodePath SkeletonModification3DFABRIK::get_target_node() const { + return target_node; +} + +int SkeletonModification3DFABRIK::get_fabrik_data_chain_length() { + return fabrik_data_chain.size(); +} + +void SkeletonModification3DFABRIK::set_fabrik_data_chain_length(int p_length) { + ERR_FAIL_COND(p_length < 0); + fabrik_data_chain.resize(p_length); + execution_error_found = false; + notify_property_list_changed(); +} + +real_t SkeletonModification3DFABRIK::get_chain_tolerance() { + return chain_tolerance; +} + +void SkeletonModification3DFABRIK::set_chain_tolerance(real_t p_tolerance) { + ERR_FAIL_COND_MSG(p_tolerance <= 0, "FABRIK chain tolerance must be more than zero!"); + chain_tolerance = p_tolerance; +} + +int SkeletonModification3DFABRIK::get_chain_max_iterations() { + return chain_max_iterations; +} +void SkeletonModification3DFABRIK::set_chain_max_iterations(int p_iterations) { + ERR_FAIL_COND_MSG(p_iterations <= 0, "FABRIK chain iterations must be at least one. Set enabled to false to disable the FABRIK chain."); + chain_max_iterations = p_iterations; +} + +// FABRIK joint data functions +String SkeletonModification3DFABRIK::get_fabrik_joint_bone_name(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, String()); + return fabrik_data_chain[p_joint_idx].bone_name; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_bone_name(int p_joint_idx, String p_bone_name) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + fabrik_data_chain[p_joint_idx].bone_name = p_bone_name; + + if (stack) { + if (stack->skeleton) { + fabrik_data_chain[p_joint_idx].bone_idx = stack->skeleton->find_bone(p_bone_name); + } + } + execution_error_found = false; + notify_property_list_changed(); +} + +int SkeletonModification3DFABRIK::get_fabrik_joint_bone_index(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, -1); + return fabrik_data_chain[p_joint_idx].bone_idx; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_bone_index(int p_joint_idx, int p_bone_idx) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ERR_FAIL_COND_MSG(p_bone_idx < 0, "Bone index is out of range: The index is too low!"); + fabrik_data_chain[p_joint_idx].bone_idx = p_bone_idx; + + if (stack) { + if (stack->skeleton) { + fabrik_data_chain[p_joint_idx].bone_name = stack->skeleton->get_bone_name(p_bone_idx); + } + } + execution_error_found = false; + notify_property_list_changed(); +} + +real_t SkeletonModification3DFABRIK::get_fabrik_joint_length(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, -1); + return fabrik_data_chain[p_joint_idx].length; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_length(int p_joint_idx, real_t p_bone_length) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ERR_FAIL_COND_MSG(p_bone_length < 0, "FABRIK joint length cannot be less than zero!"); + + if (!is_setup) { + fabrik_data_chain[p_joint_idx].length = p_bone_length; + return; + } + + if (fabrik_data_chain[p_joint_idx].auto_calculate_length) { + WARN_PRINT("FABRIK Length not set: auto calculate length is enabled for this joint!"); + fabrik_joint_auto_calculate_length(p_joint_idx); + } else { + fabrik_data_chain[p_joint_idx].length = p_bone_length; + } + + execution_error_found = false; +} + +Vector3 SkeletonModification3DFABRIK::get_fabrik_joint_magnet(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, Vector3()); + return fabrik_data_chain[p_joint_idx].magnet_position; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_magnet(int p_joint_idx, Vector3 p_magnet) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + fabrik_data_chain[p_joint_idx].magnet_position = p_magnet; +} + +bool SkeletonModification3DFABRIK::get_fabrik_joint_auto_calculate_length(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return fabrik_data_chain[p_joint_idx].auto_calculate_length; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_auto_calculate_length(int p_joint_idx, bool p_auto_calculate) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + fabrik_data_chain[p_joint_idx].auto_calculate_length = p_auto_calculate; + fabrik_joint_auto_calculate_length(p_joint_idx); + notify_property_list_changed(); +} + +void SkeletonModification3DFABRIK::fabrik_joint_auto_calculate_length(int p_joint_idx) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + if (!fabrik_data_chain[p_joint_idx].auto_calculate_length) { + return; + } + + if (!stack || !stack->skeleton || !is_setup) { + _print_execution_error(true, "Cannot auto calculate joint length: modification is not properly setup!"); + return; + } + ERR_FAIL_INDEX_MSG(fabrik_data_chain[p_joint_idx].bone_idx, stack->skeleton->get_bone_count(), + "Bone for joint " + itos(p_joint_idx) + " is not set or points to an unknown bone!"); + + if (fabrik_data_chain[p_joint_idx].use_tip_node) { // Use the tip node to update joint length. + + update_joint_tip_cache(p_joint_idx); + + Node3D *tip_node = Object::cast_to<Node3D>(ObjectDB::get_instance(fabrik_data_chain[p_joint_idx].tip_node_cache)); + ERR_FAIL_COND_MSG(!tip_node, "Tip node for joint " + itos(p_joint_idx) + "is not a Node3D-based node. Cannot calculate length..."); + ERR_FAIL_COND_MSG(!tip_node->is_inside_tree(), "Tip node for joint " + itos(p_joint_idx) + "is not in the scene tree. Cannot calculate length..."); + + Transform3D node_trans = tip_node->get_global_transform(); + node_trans = stack->skeleton->world_transform_to_global_pose(node_trans); + node_trans = stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[p_joint_idx].bone_idx, node_trans); + fabrik_data_chain[p_joint_idx].length = node_trans.origin.length(); + } else { // Use child bone(s) to update joint length, if possible + Vector<int> bone_children = stack->skeleton->get_bone_children(fabrik_data_chain[p_joint_idx].bone_idx); + if (bone_children.size() <= 0) { + ERR_FAIL_MSG("Cannot calculate length for joint " + itos(p_joint_idx) + "joint uses leaf bone. \nPlease manually set the bone length or use a tip node!"); + return; + } + + real_t final_length = 0; + for (int i = 0; i < bone_children.size(); i++) { + Transform3D child_transform = stack->skeleton->get_bone_global_pose(bone_children[i]); + final_length += stack->skeleton->global_pose_to_local_pose(fabrik_data_chain[p_joint_idx].bone_idx, child_transform).origin.length(); + } + fabrik_data_chain[p_joint_idx].length = final_length / bone_children.size(); + } + execution_error_found = false; + notify_property_list_changed(); +} + +bool SkeletonModification3DFABRIK::get_fabrik_joint_use_tip_node(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return fabrik_data_chain[p_joint_idx].use_tip_node; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_use_tip_node(int p_joint_idx, bool p_use_tip_node) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + fabrik_data_chain[p_joint_idx].use_tip_node = p_use_tip_node; + notify_property_list_changed(); +} + +NodePath SkeletonModification3DFABRIK::get_fabrik_joint_tip_node(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, NodePath()); + return fabrik_data_chain[p_joint_idx].tip_node; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_tip_node(int p_joint_idx, NodePath p_tip_node) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + fabrik_data_chain[p_joint_idx].tip_node = p_tip_node; + update_joint_tip_cache(p_joint_idx); +} + +bool SkeletonModification3DFABRIK::get_fabrik_joint_use_target_basis(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return fabrik_data_chain[p_joint_idx].use_target_basis; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_use_target_basis(int p_joint_idx, bool p_use_target_basis) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + fabrik_data_chain[p_joint_idx].use_target_basis = p_use_target_basis; +} + +real_t SkeletonModification3DFABRIK::get_fabrik_joint_roll(int p_joint_idx) const { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, 0.0); + return fabrik_data_chain[p_joint_idx].roll; +} + +void SkeletonModification3DFABRIK::set_fabrik_joint_roll(int p_joint_idx, real_t p_roll) { + const int bone_chain_size = fabrik_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + fabrik_data_chain[p_joint_idx].roll = p_roll; +} + +void SkeletonModification3DFABRIK::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_target_node", "target_nodepath"), &SkeletonModification3DFABRIK::set_target_node); + ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonModification3DFABRIK::get_target_node); + ClassDB::bind_method(D_METHOD("set_fabrik_data_chain_length", "length"), &SkeletonModification3DFABRIK::set_fabrik_data_chain_length); + ClassDB::bind_method(D_METHOD("get_fabrik_data_chain_length"), &SkeletonModification3DFABRIK::get_fabrik_data_chain_length); + ClassDB::bind_method(D_METHOD("set_chain_tolerance", "tolerance"), &SkeletonModification3DFABRIK::set_chain_tolerance); + ClassDB::bind_method(D_METHOD("get_chain_tolerance"), &SkeletonModification3DFABRIK::get_chain_tolerance); + ClassDB::bind_method(D_METHOD("set_chain_max_iterations", "max_iterations"), &SkeletonModification3DFABRIK::set_chain_max_iterations); + ClassDB::bind_method(D_METHOD("get_chain_max_iterations"), &SkeletonModification3DFABRIK::get_chain_max_iterations); + + // FABRIK joint data functions + ClassDB::bind_method(D_METHOD("get_fabrik_joint_bone_name", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_bone_name); + ClassDB::bind_method(D_METHOD("set_fabrik_joint_bone_name", "joint_idx", "bone_name"), &SkeletonModification3DFABRIK::set_fabrik_joint_bone_name); + ClassDB::bind_method(D_METHOD("get_fabrik_joint_bone_index", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_bone_index); + ClassDB::bind_method(D_METHOD("set_fabrik_joint_bone_index", "joint_idx", "bone_index"), &SkeletonModification3DFABRIK::set_fabrik_joint_bone_index); + ClassDB::bind_method(D_METHOD("get_fabrik_joint_length", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_length); + ClassDB::bind_method(D_METHOD("set_fabrik_joint_length", "joint_idx", "length"), &SkeletonModification3DFABRIK::set_fabrik_joint_length); + ClassDB::bind_method(D_METHOD("get_fabrik_joint_magnet", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_magnet); + ClassDB::bind_method(D_METHOD("set_fabrik_joint_magnet", "joint_idx", "magnet_position"), &SkeletonModification3DFABRIK::set_fabrik_joint_magnet); + ClassDB::bind_method(D_METHOD("get_fabrik_joint_auto_calculate_length", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_auto_calculate_length); + ClassDB::bind_method(D_METHOD("set_fabrik_joint_auto_calculate_length", "joint_idx", "auto_calculate_length"), &SkeletonModification3DFABRIK::set_fabrik_joint_auto_calculate_length); + ClassDB::bind_method(D_METHOD("fabrik_joint_auto_calculate_length", "joint_idx"), &SkeletonModification3DFABRIK::fabrik_joint_auto_calculate_length); + ClassDB::bind_method(D_METHOD("get_fabrik_joint_use_tip_node", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_use_tip_node); + ClassDB::bind_method(D_METHOD("set_fabrik_joint_use_tip_node", "joint_idx", "use_tip_node"), &SkeletonModification3DFABRIK::set_fabrik_joint_use_tip_node); + ClassDB::bind_method(D_METHOD("get_fabrik_joint_tip_node", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_tip_node); + ClassDB::bind_method(D_METHOD("set_fabrik_joint_tip_node", "joint_idx", "tip_node"), &SkeletonModification3DFABRIK::set_fabrik_joint_tip_node); + ClassDB::bind_method(D_METHOD("get_fabrik_joint_use_target_basis", "joint_idx"), &SkeletonModification3DFABRIK::get_fabrik_joint_use_target_basis); + ClassDB::bind_method(D_METHOD("set_fabrik_joint_use_target_basis", "joint_idx", "use_target_basis"), &SkeletonModification3DFABRIK::set_fabrik_joint_use_target_basis); + + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_nodepath", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D"), "set_target_node", "get_target_node"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "fabrik_data_chain_length", PROPERTY_HINT_RANGE, "0,100,1"), "set_fabrik_data_chain_length", "get_fabrik_data_chain_length"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "chain_tolerance", PROPERTY_HINT_RANGE, "0,100,0.001"), "set_chain_tolerance", "get_chain_tolerance"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "chain_max_iterations", PROPERTY_HINT_RANGE, "1,50,1"), "set_chain_max_iterations", "get_chain_max_iterations"); +} + +SkeletonModification3DFABRIK::SkeletonModification3DFABRIK() { + stack = nullptr; + is_setup = false; + enabled = true; +} + +SkeletonModification3DFABRIK::~SkeletonModification3DFABRIK() { +} diff --git a/scene/resources/skeleton_modification_3d_fabrik.h b/scene/resources/skeleton_modification_3d_fabrik.h new file mode 100644 index 0000000000..9b5da883d4 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_fabrik.h @@ -0,0 +1,122 @@ +/*************************************************************************/ +/* skeleton_modification_3d_fabrik.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "core/templates/local_vector.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +#ifndef SKELETONMODIFICATION3DFABRIK_H +#define SKELETONMODIFICATION3DFABRIK_H + +class SkeletonModification3DFABRIK : public SkeletonModification3D { + GDCLASS(SkeletonModification3DFABRIK, SkeletonModification3D); + +private: + struct FABRIK_Joint_Data { + String bone_name = ""; + int bone_idx = -1; + real_t length = -1; + Vector3 magnet_position = Vector3(0, 0, 0); + + bool auto_calculate_length = true; + bool use_tip_node = false; + NodePath tip_node = NodePath(); + ObjectID tip_node_cache; + + bool use_target_basis = false; + real_t roll = 0; + }; + + LocalVector<FABRIK_Joint_Data> fabrik_data_chain; + NodePath target_node; + ObjectID target_node_cache; + + real_t chain_tolerance = 0.01; + int chain_max_iterations = 10; + int chain_iterations = 0; + + void update_target_cache(); + void update_joint_tip_cache(int p_joint_idx); + + int final_joint_idx = 0; + Transform3D target_global_pose = Transform3D(); + Transform3D origin_global_pose = Transform3D(); + + void chain_backwards(); + void chain_forwards(); + void chain_apply(); + +protected: + static void _bind_methods(); + bool _get(const StringName &p_path, Variant &r_ret) const; + bool _set(const StringName &p_path, const Variant &p_value); + void _get_property_list(List<PropertyInfo> *p_list) const; + +public: + virtual void _execute(real_t p_delta) override; + virtual void _setup_modification(SkeletonModificationStack3D *p_stack) override; + + void set_target_node(const NodePath &p_target_node); + NodePath get_target_node() const; + + int get_fabrik_data_chain_length(); + void set_fabrik_data_chain_length(int p_new_length); + + real_t get_chain_tolerance(); + void set_chain_tolerance(real_t p_tolerance); + + int get_chain_max_iterations(); + void set_chain_max_iterations(int p_iterations); + + String get_fabrik_joint_bone_name(int p_joint_idx) const; + void set_fabrik_joint_bone_name(int p_joint_idx, String p_bone_name); + int get_fabrik_joint_bone_index(int p_joint_idx) const; + void set_fabrik_joint_bone_index(int p_joint_idx, int p_bone_idx); + real_t get_fabrik_joint_length(int p_joint_idx) const; + void set_fabrik_joint_length(int p_joint_idx, real_t p_bone_length); + Vector3 get_fabrik_joint_magnet(int p_joint_idx) const; + void set_fabrik_joint_magnet(int p_joint_idx, Vector3 p_magnet); + bool get_fabrik_joint_auto_calculate_length(int p_joint_idx) const; + void set_fabrik_joint_auto_calculate_length(int p_joint_idx, bool p_auto_calculate); + void fabrik_joint_auto_calculate_length(int p_joint_idx); + bool get_fabrik_joint_use_tip_node(int p_joint_idx) const; + void set_fabrik_joint_use_tip_node(int p_joint_idx, bool p_use_tip_node); + NodePath get_fabrik_joint_tip_node(int p_joint_idx) const; + void set_fabrik_joint_tip_node(int p_joint_idx, NodePath p_tip_node); + bool get_fabrik_joint_use_target_basis(int p_joint_idx) const; + void set_fabrik_joint_use_target_basis(int p_joint_idx, bool p_use_basis); + real_t get_fabrik_joint_roll(int p_joint_idx) const; + void set_fabrik_joint_roll(int p_joint_idx, real_t p_roll); + + SkeletonModification3DFABRIK(); + ~SkeletonModification3DFABRIK(); +}; + +#endif //SKELETONMODIFICATION3DFABRIK_H diff --git a/scene/resources/skeleton_modification_3d_jiggle.cpp b/scene/resources/skeleton_modification_3d_jiggle.cpp new file mode 100644 index 0000000000..1fb7dad2ad --- /dev/null +++ b/scene/resources/skeleton_modification_3d_jiggle.cpp @@ -0,0 +1,573 @@ +/*************************************************************************/ +/* skeleton_modification_3d_jiggle.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/resources/skeleton_modification_3d_jiggle.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +bool SkeletonModification3DJiggle::_set(const StringName &p_path, const Variant &p_value) { + String path = p_path; + + if (path.begins_with("joint_data/")) { + const int jiggle_size = jiggle_data_chain.size(); + int which = path.get_slicec('/', 1).to_int(); + String what = path.get_slicec('/', 2); + ERR_FAIL_INDEX_V(which, jiggle_size, false); + + if (what == "bone_name") { + set_jiggle_joint_bone_name(which, p_value); + } else if (what == "bone_index") { + set_jiggle_joint_bone_index(which, p_value); + } else if (what == "override_defaults") { + set_jiggle_joint_override(which, p_value); + } else if (what == "stiffness") { + set_jiggle_joint_stiffness(which, p_value); + } else if (what == "mass") { + set_jiggle_joint_mass(which, p_value); + } else if (what == "damping") { + set_jiggle_joint_damping(which, p_value); + } else if (what == "use_gravity") { + set_jiggle_joint_use_gravity(which, p_value); + } else if (what == "gravity") { + set_jiggle_joint_gravity(which, p_value); + } else if (what == "roll") { + set_jiggle_joint_roll(which, Math::deg2rad(real_t(p_value))); + } + return true; + } else { + if (path == "use_colliders") { + set_use_colliders(p_value); + } else if (path == "collision_mask") { + set_collision_mask(p_value); + } + return true; + } + return true; +} + +bool SkeletonModification3DJiggle::_get(const StringName &p_path, Variant &r_ret) const { + String path = p_path; + + if (path.begins_with("joint_data/")) { + const int jiggle_size = jiggle_data_chain.size(); + int which = path.get_slicec('/', 1).to_int(); + String what = path.get_slicec('/', 2); + ERR_FAIL_INDEX_V(which, jiggle_size, false); + + if (what == "bone_name") { + r_ret = get_jiggle_joint_bone_name(which); + } else if (what == "bone_index") { + r_ret = get_jiggle_joint_bone_index(which); + } else if (what == "override_defaults") { + r_ret = get_jiggle_joint_override(which); + } else if (what == "stiffness") { + r_ret = get_jiggle_joint_stiffness(which); + } else if (what == "mass") { + r_ret = get_jiggle_joint_mass(which); + } else if (what == "damping") { + r_ret = get_jiggle_joint_damping(which); + } else if (what == "use_gravity") { + r_ret = get_jiggle_joint_use_gravity(which); + } else if (what == "gravity") { + r_ret = get_jiggle_joint_gravity(which); + } else if (what == "roll") { + r_ret = Math::rad2deg(get_jiggle_joint_roll(which)); + } + return true; + } else { + if (path == "use_colliders") { + r_ret = get_use_colliders(); + } else if (path == "collision_mask") { + r_ret = get_collision_mask(); + } + return true; + } + return true; +} + +void SkeletonModification3DJiggle::_get_property_list(List<PropertyInfo> *p_list) const { + p_list->push_back(PropertyInfo(Variant::BOOL, "use_colliders", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + if (use_colliders) { + p_list->push_back(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS, "", PROPERTY_USAGE_DEFAULT)); + } + + for (uint32_t i = 0; i < jiggle_data_chain.size(); i++) { + String base_string = "joint_data/" + itos(i) + "/"; + + p_list->push_back(PropertyInfo(Variant::STRING_NAME, base_string + "bone_name", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::INT, base_string + "bone_index", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "roll", PROPERTY_HINT_RANGE, "-360,360,0.01", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::BOOL, base_string + "override_defaults", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + + if (jiggle_data_chain[i].override_defaults) { + p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "stiffness", PROPERTY_HINT_RANGE, "0, 1000, 0.01", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "mass", PROPERTY_HINT_RANGE, "0, 1000, 0.01", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::FLOAT, base_string + "damping", PROPERTY_HINT_RANGE, "0, 1, 0.01", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::BOOL, base_string + "use_gravity", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + if (jiggle_data_chain[i].use_gravity) { + p_list->push_back(PropertyInfo(Variant::VECTOR3, base_string + "gravity", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + } + } + } +} + +void SkeletonModification3DJiggle::_execute(real_t p_delta) { + ERR_FAIL_COND_MSG(!stack || !is_setup || stack->skeleton == nullptr, + "Modification is not setup and therefore cannot execute!"); + if (!enabled) { + return; + } + if (target_node_cache.is_null()) { + _print_execution_error(true, "Target cache is out of date. Attempting to update..."); + update_cache(); + return; + } + Node3D *target = Object::cast_to<Node3D>(ObjectDB::get_instance(target_node_cache)); + _print_execution_error(!target || !target->is_inside_tree(), "Target node is not in the scene tree. Cannot execute modification!"); + + for (uint32_t i = 0; i < jiggle_data_chain.size(); i++) { + _execute_jiggle_joint(i, target, p_delta); + } + + execution_error_found = false; +} + +void SkeletonModification3DJiggle::_execute_jiggle_joint(int p_joint_idx, Node3D *p_target, real_t p_delta) { + // Adopted from: https://wiki.unity3d.com/index.php/JiggleBone + // With modifications by TwistedTwigleg. + + if (jiggle_data_chain[p_joint_idx].bone_idx <= -2) { + jiggle_data_chain[p_joint_idx].bone_idx = stack->skeleton->find_bone(jiggle_data_chain[p_joint_idx].bone_name); + } + if (_print_execution_error( + jiggle_data_chain[p_joint_idx].bone_idx < 0 || jiggle_data_chain[p_joint_idx].bone_idx > stack->skeleton->get_bone_count(), + "Jiggle joint " + itos(p_joint_idx) + " bone index is invald. Cannot execute modification!")) { + return; + } + + Transform3D new_bone_trans = stack->skeleton->local_pose_to_global_pose(jiggle_data_chain[p_joint_idx].bone_idx, stack->skeleton->get_bone_local_pose_override(jiggle_data_chain[p_joint_idx].bone_idx)); + Vector3 target_position = stack->skeleton->world_transform_to_global_pose(p_target->get_global_transform()).origin; + + jiggle_data_chain[p_joint_idx].force = (target_position - jiggle_data_chain[p_joint_idx].dynamic_position) * jiggle_data_chain[p_joint_idx].stiffness * p_delta; + + if (jiggle_data_chain[p_joint_idx].use_gravity) { + Vector3 gravity_to_apply = new_bone_trans.basis.inverse().xform(jiggle_data_chain[p_joint_idx].gravity); + jiggle_data_chain[p_joint_idx].force += gravity_to_apply * p_delta; + } + + jiggle_data_chain[p_joint_idx].acceleration = jiggle_data_chain[p_joint_idx].force / jiggle_data_chain[p_joint_idx].mass; + jiggle_data_chain[p_joint_idx].velocity += jiggle_data_chain[p_joint_idx].acceleration * (1 - jiggle_data_chain[p_joint_idx].damping); + + jiggle_data_chain[p_joint_idx].dynamic_position += jiggle_data_chain[p_joint_idx].velocity + jiggle_data_chain[p_joint_idx].force; + jiggle_data_chain[p_joint_idx].dynamic_position += new_bone_trans.origin - jiggle_data_chain[p_joint_idx].last_position; + jiggle_data_chain[p_joint_idx].last_position = new_bone_trans.origin; + + // Collision detection/response + if (use_colliders) { + if (execution_mode == SkeletonModificationStack3D::EXECUTION_MODE::execution_mode_physics_process) { + Ref<World3D> world_3d = stack->skeleton->get_world_3d(); + ERR_FAIL_COND(world_3d.is_null()); + PhysicsDirectSpaceState3D *space_state = PhysicsServer3D::get_singleton()->space_get_direct_state(world_3d->get_space()); + PhysicsDirectSpaceState3D::RayResult ray_result; + + // Convert to world transforms, which is what the physics server needs + Transform3D new_bone_trans_world = stack->skeleton->global_pose_to_world_transform(new_bone_trans); + Transform3D dynamic_position_world = stack->skeleton->global_pose_to_world_transform(Transform3D(Basis(), jiggle_data_chain[p_joint_idx].dynamic_position)); + + bool ray_hit = space_state->intersect_ray(new_bone_trans_world.origin, dynamic_position_world.get_origin(), + ray_result, Set<RID>(), collision_mask); + + if (ray_hit) { + jiggle_data_chain[p_joint_idx].dynamic_position = jiggle_data_chain[p_joint_idx].last_noncollision_position; + jiggle_data_chain[p_joint_idx].acceleration = Vector3(0, 0, 0); + jiggle_data_chain[p_joint_idx].velocity = Vector3(0, 0, 0); + } else { + jiggle_data_chain[p_joint_idx].last_noncollision_position = jiggle_data_chain[p_joint_idx].dynamic_position; + } + + } else { + WARN_PRINT_ONCE("Jiggle modifier: You cannot detect colliders without the stack mode being set to _physics_process!"); + } + } + + // Get the forward direction that the basis is facing in right now. + stack->skeleton->update_bone_rest_forward_vector(jiggle_data_chain[p_joint_idx].bone_idx); + Vector3 forward_vector = stack->skeleton->get_bone_axis_forward_vector(jiggle_data_chain[p_joint_idx].bone_idx); + + // Rotate the bone using the dynamic position! + new_bone_trans.basis.rotate_to_align(forward_vector, new_bone_trans.origin.direction_to(jiggle_data_chain[p_joint_idx].dynamic_position)); + + // Roll + new_bone_trans.basis.rotate_local(forward_vector, jiggle_data_chain[p_joint_idx].roll); + + new_bone_trans = stack->skeleton->global_pose_to_local_pose(jiggle_data_chain[p_joint_idx].bone_idx, new_bone_trans); + stack->skeleton->set_bone_local_pose_override(jiggle_data_chain[p_joint_idx].bone_idx, new_bone_trans, stack->strength, true); + stack->skeleton->force_update_bone_children_transforms(jiggle_data_chain[p_joint_idx].bone_idx); +} + +void SkeletonModification3DJiggle::_update_jiggle_joint_data() { + for (uint32_t i = 0; i < jiggle_data_chain.size(); i++) { + if (!jiggle_data_chain[i].override_defaults) { + set_jiggle_joint_stiffness(i, stiffness); + set_jiggle_joint_mass(i, mass); + set_jiggle_joint_damping(i, damping); + set_jiggle_joint_use_gravity(i, use_gravity); + set_jiggle_joint_gravity(i, gravity); + } + } +} + +void SkeletonModification3DJiggle::_setup_modification(SkeletonModificationStack3D *p_stack) { + stack = p_stack; + + if (stack) { + is_setup = true; + execution_error_found = false; + + if (stack->skeleton) { + for (uint32_t i = 0; i < jiggle_data_chain.size(); i++) { + int bone_idx = jiggle_data_chain[i].bone_idx; + if (bone_idx > 0 && bone_idx < stack->skeleton->get_bone_count()) { + jiggle_data_chain[i].dynamic_position = stack->skeleton->local_pose_to_global_pose(bone_idx, stack->skeleton->get_bone_local_pose_override(bone_idx)).origin; + } + } + } + + update_cache(); + } +} + +void SkeletonModification3DJiggle::update_cache() { + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update target cache: modification is not properly setup!"); + return; + } + + target_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree()) { + if (stack->skeleton->has_node(target_node)) { + Node *node = stack->skeleton->get_node(target_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update target cache: node is this modification's skeleton or cannot be found!"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update target cache: node is not in the scene tree!"); + target_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DJiggle::set_target_node(const NodePath &p_target_node) { + target_node = p_target_node; + update_cache(); +} + +NodePath SkeletonModification3DJiggle::get_target_node() const { + return target_node; +} + +void SkeletonModification3DJiggle::set_stiffness(real_t p_stiffness) { + ERR_FAIL_COND_MSG(p_stiffness < 0, "Stiffness cannot be set to a negative value!"); + stiffness = p_stiffness; + _update_jiggle_joint_data(); +} + +real_t SkeletonModification3DJiggle::get_stiffness() const { + return stiffness; +} + +void SkeletonModification3DJiggle::set_mass(real_t p_mass) { + ERR_FAIL_COND_MSG(p_mass < 0, "Mass cannot be set to a negative value!"); + mass = p_mass; + _update_jiggle_joint_data(); +} + +real_t SkeletonModification3DJiggle::get_mass() const { + return mass; +} + +void SkeletonModification3DJiggle::set_damping(real_t p_damping) { + ERR_FAIL_COND_MSG(p_damping < 0, "Damping cannot be set to a negative value!"); + ERR_FAIL_COND_MSG(p_damping > 1, "Damping cannot be more than one!"); + damping = p_damping; + _update_jiggle_joint_data(); +} + +real_t SkeletonModification3DJiggle::get_damping() const { + return damping; +} + +void SkeletonModification3DJiggle::set_use_gravity(bool p_use_gravity) { + use_gravity = p_use_gravity; + _update_jiggle_joint_data(); +} + +bool SkeletonModification3DJiggle::get_use_gravity() const { + return use_gravity; +} + +void SkeletonModification3DJiggle::set_gravity(Vector3 p_gravity) { + gravity = p_gravity; + _update_jiggle_joint_data(); +} + +Vector3 SkeletonModification3DJiggle::get_gravity() const { + return gravity; +} + +void SkeletonModification3DJiggle::set_use_colliders(bool p_use_collider) { + use_colliders = p_use_collider; + notify_property_list_changed(); +} + +bool SkeletonModification3DJiggle::get_use_colliders() const { + return use_colliders; +} + +void SkeletonModification3DJiggle::set_collision_mask(int p_mask) { + collision_mask = p_mask; +} + +int SkeletonModification3DJiggle::get_collision_mask() const { + return collision_mask; +} + +// Jiggle joint data functions +int SkeletonModification3DJiggle::get_jiggle_data_chain_length() { + return jiggle_data_chain.size(); +} + +void SkeletonModification3DJiggle::set_jiggle_data_chain_length(int p_length) { + ERR_FAIL_COND(p_length < 0); + jiggle_data_chain.resize(p_length); + execution_error_found = false; + notify_property_list_changed(); +} + +void SkeletonModification3DJiggle::set_jiggle_joint_bone_name(int p_joint_idx, String p_name) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + + jiggle_data_chain[p_joint_idx].bone_name = p_name; + if (stack && stack->skeleton) { + jiggle_data_chain[p_joint_idx].bone_idx = stack->skeleton->find_bone(p_name); + } + execution_error_found = false; + notify_property_list_changed(); +} + +String SkeletonModification3DJiggle::get_jiggle_joint_bone_name(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, ""); + return jiggle_data_chain[p_joint_idx].bone_name; +} + +int SkeletonModification3DJiggle::get_jiggle_joint_bone_index(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, -1); + return jiggle_data_chain[p_joint_idx].bone_idx; +} + +void SkeletonModification3DJiggle::set_jiggle_joint_bone_index(int p_joint_idx, int p_bone_idx) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + ERR_FAIL_COND_MSG(p_bone_idx < 0, "Bone index is out of range: The index is too low!"); + jiggle_data_chain[p_joint_idx].bone_idx = p_bone_idx; + + if (stack) { + if (stack->skeleton) { + jiggle_data_chain[p_joint_idx].bone_name = stack->skeleton->get_bone_name(p_bone_idx); + } + } + execution_error_found = false; + notify_property_list_changed(); +} + +void SkeletonModification3DJiggle::set_jiggle_joint_override(int p_joint_idx, bool p_override) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + jiggle_data_chain[p_joint_idx].override_defaults = p_override; + _update_jiggle_joint_data(); + notify_property_list_changed(); +} + +bool SkeletonModification3DJiggle::get_jiggle_joint_override(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return jiggle_data_chain[p_joint_idx].override_defaults; +} + +void SkeletonModification3DJiggle::set_jiggle_joint_stiffness(int p_joint_idx, real_t p_stiffness) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_COND_MSG(p_stiffness < 0, "Stiffness cannot be set to a negative value!"); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + jiggle_data_chain[p_joint_idx].stiffness = p_stiffness; +} + +real_t SkeletonModification3DJiggle::get_jiggle_joint_stiffness(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, -1); + return jiggle_data_chain[p_joint_idx].stiffness; +} + +void SkeletonModification3DJiggle::set_jiggle_joint_mass(int p_joint_idx, real_t p_mass) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_COND_MSG(p_mass < 0, "Mass cannot be set to a negative value!"); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + jiggle_data_chain[p_joint_idx].mass = p_mass; +} + +real_t SkeletonModification3DJiggle::get_jiggle_joint_mass(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, -1); + return jiggle_data_chain[p_joint_idx].mass; +} + +void SkeletonModification3DJiggle::set_jiggle_joint_damping(int p_joint_idx, real_t p_damping) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_COND_MSG(p_damping < 0, "Damping cannot be set to a negative value!"); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + jiggle_data_chain[p_joint_idx].damping = p_damping; +} + +real_t SkeletonModification3DJiggle::get_jiggle_joint_damping(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, -1); + return jiggle_data_chain[p_joint_idx].damping; +} + +void SkeletonModification3DJiggle::set_jiggle_joint_use_gravity(int p_joint_idx, bool p_use_gravity) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + jiggle_data_chain[p_joint_idx].use_gravity = p_use_gravity; + notify_property_list_changed(); +} + +bool SkeletonModification3DJiggle::get_jiggle_joint_use_gravity(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, false); + return jiggle_data_chain[p_joint_idx].use_gravity; +} + +void SkeletonModification3DJiggle::set_jiggle_joint_gravity(int p_joint_idx, Vector3 p_gravity) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + jiggle_data_chain[p_joint_idx].gravity = p_gravity; +} + +Vector3 SkeletonModification3DJiggle::get_jiggle_joint_gravity(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, Vector3(0, 0, 0)); + return jiggle_data_chain[p_joint_idx].gravity; +} + +void SkeletonModification3DJiggle::set_jiggle_joint_roll(int p_joint_idx, real_t p_roll) { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX(p_joint_idx, bone_chain_size); + jiggle_data_chain[p_joint_idx].roll = p_roll; +} + +real_t SkeletonModification3DJiggle::get_jiggle_joint_roll(int p_joint_idx) const { + const int bone_chain_size = jiggle_data_chain.size(); + ERR_FAIL_INDEX_V(p_joint_idx, bone_chain_size, 0.0); + return jiggle_data_chain[p_joint_idx].roll; +} + +void SkeletonModification3DJiggle::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_target_node", "target_nodepath"), &SkeletonModification3DJiggle::set_target_node); + ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonModification3DJiggle::get_target_node); + + ClassDB::bind_method(D_METHOD("set_jiggle_data_chain_length", "length"), &SkeletonModification3DJiggle::set_jiggle_data_chain_length); + ClassDB::bind_method(D_METHOD("get_jiggle_data_chain_length"), &SkeletonModification3DJiggle::get_jiggle_data_chain_length); + + ClassDB::bind_method(D_METHOD("set_stiffness", "stiffness"), &SkeletonModification3DJiggle::set_stiffness); + ClassDB::bind_method(D_METHOD("get_stiffness"), &SkeletonModification3DJiggle::get_stiffness); + ClassDB::bind_method(D_METHOD("set_mass", "mass"), &SkeletonModification3DJiggle::set_mass); + ClassDB::bind_method(D_METHOD("get_mass"), &SkeletonModification3DJiggle::get_mass); + ClassDB::bind_method(D_METHOD("set_damping", "damping"), &SkeletonModification3DJiggle::set_damping); + ClassDB::bind_method(D_METHOD("get_damping"), &SkeletonModification3DJiggle::get_damping); + ClassDB::bind_method(D_METHOD("set_use_gravity", "use_gravity"), &SkeletonModification3DJiggle::set_use_gravity); + ClassDB::bind_method(D_METHOD("get_use_gravity"), &SkeletonModification3DJiggle::get_use_gravity); + ClassDB::bind_method(D_METHOD("set_gravity", "gravity"), &SkeletonModification3DJiggle::set_gravity); + ClassDB::bind_method(D_METHOD("get_gravity"), &SkeletonModification3DJiggle::get_gravity); + + ClassDB::bind_method(D_METHOD("set_use_colliders", "use_colliders"), &SkeletonModification3DJiggle::set_use_colliders); + ClassDB::bind_method(D_METHOD("get_use_colliders"), &SkeletonModification3DJiggle::get_use_colliders); + ClassDB::bind_method(D_METHOD("set_collision_mask", "mask"), &SkeletonModification3DJiggle::set_collision_mask); + ClassDB::bind_method(D_METHOD("get_collision_mask"), &SkeletonModification3DJiggle::get_collision_mask); + + // Jiggle joint data functions + ClassDB::bind_method(D_METHOD("set_jiggle_joint_bone_name", "joint_idx", "name"), &SkeletonModification3DJiggle::set_jiggle_joint_bone_name); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_bone_name", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_bone_name); + ClassDB::bind_method(D_METHOD("set_jiggle_joint_bone_index", "joint_idx", "bone_idx"), &SkeletonModification3DJiggle::set_jiggle_joint_bone_index); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_bone_index", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_bone_index); + ClassDB::bind_method(D_METHOD("set_jiggle_joint_override", "joint_idx", "override"), &SkeletonModification3DJiggle::set_jiggle_joint_override); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_override", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_override); + ClassDB::bind_method(D_METHOD("set_jiggle_joint_stiffness", "joint_idx", "stiffness"), &SkeletonModification3DJiggle::set_jiggle_joint_stiffness); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_stiffness", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_stiffness); + ClassDB::bind_method(D_METHOD("set_jiggle_joint_mass", "joint_idx", "mass"), &SkeletonModification3DJiggle::set_jiggle_joint_mass); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_mass", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_mass); + ClassDB::bind_method(D_METHOD("set_jiggle_joint_damping", "joint_idx", "damping"), &SkeletonModification3DJiggle::set_jiggle_joint_damping); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_damping", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_damping); + ClassDB::bind_method(D_METHOD("set_jiggle_joint_use_gravity", "joint_idx", "use_gravity"), &SkeletonModification3DJiggle::set_jiggle_joint_use_gravity); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_use_gravity", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_use_gravity); + ClassDB::bind_method(D_METHOD("set_jiggle_joint_gravity", "joint_idx", "gravity"), &SkeletonModification3DJiggle::set_jiggle_joint_gravity); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_gravity", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_gravity); + ClassDB::bind_method(D_METHOD("set_jiggle_joint_roll", "joint_idx", "roll"), &SkeletonModification3DJiggle::set_jiggle_joint_roll); + ClassDB::bind_method(D_METHOD("get_jiggle_joint_roll", "joint_idx"), &SkeletonModification3DJiggle::get_jiggle_joint_roll); + + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_nodepath", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D"), "set_target_node", "get_target_node"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "jiggle_data_chain_length", PROPERTY_HINT_RANGE, "0,100,1"), "set_jiggle_data_chain_length", "get_jiggle_data_chain_length"); + ADD_GROUP("Default Joint Settings", ""); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "stiffness"), "set_stiffness", "get_stiffness"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mass"), "set_mass", "get_mass"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "damping", PROPERTY_HINT_RANGE, "0, 1, 0.01"), "set_damping", "get_damping"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_gravity"), "set_use_gravity", "get_use_gravity"); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "gravity"), "set_gravity", "get_gravity"); + ADD_GROUP("", ""); +} + +SkeletonModification3DJiggle::SkeletonModification3DJiggle() { + stack = nullptr; + is_setup = false; + jiggle_data_chain = Vector<Jiggle_Joint_Data>(); + stiffness = 3; + mass = 0.75; + damping = 0.75; + use_gravity = false; + gravity = Vector3(0, -6.0, 0); + enabled = true; +} + +SkeletonModification3DJiggle::~SkeletonModification3DJiggle() { +} diff --git a/scene/resources/skeleton_modification_3d_jiggle.h b/scene/resources/skeleton_modification_3d_jiggle.h new file mode 100644 index 0000000000..c210c8fa73 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_jiggle.h @@ -0,0 +1,138 @@ +/*************************************************************************/ +/* skeleton_modification_3d_jiggle.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "core/templates/local_vector.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +#ifndef SKELETONMODIFICATION3DJIGGLE_H +#define SKELETONMODIFICATION3DJIGGLE_H + +class SkeletonModification3DJiggle : public SkeletonModification3D { + GDCLASS(SkeletonModification3DJiggle, SkeletonModification3D); + +private: + struct Jiggle_Joint_Data { + String bone_name = ""; + int bone_idx = -1; + + bool override_defaults = false; + real_t stiffness = 3; + real_t mass = 0.75; + real_t damping = 0.75; + bool use_gravity = false; + Vector3 gravity = Vector3(0, -6.0, 0); + real_t roll = 0; + + Vector3 cached_rotation = Vector3(0, 0, 0); + Vector3 force = Vector3(0, 0, 0); + Vector3 acceleration = Vector3(0, 0, 0); + Vector3 velocity = Vector3(0, 0, 0); + Vector3 last_position = Vector3(0, 0, 0); + Vector3 dynamic_position = Vector3(0, 0, 0); + + Vector3 last_noncollision_position = Vector3(0, 0, 0); + }; + + NodePath target_node; + ObjectID target_node_cache; + LocalVector<Jiggle_Joint_Data> jiggle_data_chain; + + real_t stiffness = 3; + real_t mass = 0.75; + real_t damping = 0.75; + bool use_gravity = false; + Vector3 gravity = Vector3(0, -6.0, 0); + + bool use_colliders = false; + uint32_t collision_mask = 1; + + void update_cache(); + void _execute_jiggle_joint(int p_joint_idx, Node3D *p_target, real_t p_delta); + void _update_jiggle_joint_data(); + +protected: + static void _bind_methods(); + bool _get(const StringName &p_path, Variant &r_ret) const; + bool _set(const StringName &p_path, const Variant &p_value); + void _get_property_list(List<PropertyInfo> *p_list) const; + +public: + virtual void _execute(real_t p_delta) override; + virtual void _setup_modification(SkeletonModificationStack3D *p_stack) override; + + void set_target_node(const NodePath &p_target_node); + NodePath get_target_node() const; + + void set_stiffness(real_t p_stiffness); + real_t get_stiffness() const; + void set_mass(real_t p_mass); + real_t get_mass() const; + void set_damping(real_t p_damping); + real_t get_damping() const; + + void set_use_gravity(bool p_use_gravity); + bool get_use_gravity() const; + void set_gravity(Vector3 p_gravity); + Vector3 get_gravity() const; + + void set_use_colliders(bool p_use_colliders); + bool get_use_colliders() const; + void set_collision_mask(int p_mask); + int get_collision_mask() const; + + int get_jiggle_data_chain_length(); + void set_jiggle_data_chain_length(int p_new_length); + + void set_jiggle_joint_bone_name(int p_joint_idx, String p_name); + String get_jiggle_joint_bone_name(int p_joint_idx) const; + void set_jiggle_joint_bone_index(int p_joint_idx, int p_idx); + int get_jiggle_joint_bone_index(int p_joint_idx) const; + + void set_jiggle_joint_override(int p_joint_idx, bool p_override); + bool get_jiggle_joint_override(int p_joint_idx) const; + void set_jiggle_joint_stiffness(int p_joint_idx, real_t p_stiffness); + real_t get_jiggle_joint_stiffness(int p_joint_idx) const; + void set_jiggle_joint_mass(int p_joint_idx, real_t p_mass); + real_t get_jiggle_joint_mass(int p_joint_idx) const; + void set_jiggle_joint_damping(int p_joint_idx, real_t p_damping); + real_t get_jiggle_joint_damping(int p_joint_idx) const; + void set_jiggle_joint_use_gravity(int p_joint_idx, bool p_use_gravity); + bool get_jiggle_joint_use_gravity(int p_joint_idx) const; + void set_jiggle_joint_gravity(int p_joint_idx, Vector3 p_gravity); + Vector3 get_jiggle_joint_gravity(int p_joint_idx) const; + void set_jiggle_joint_roll(int p_joint_idx, real_t p_roll); + real_t get_jiggle_joint_roll(int p_joint_idx) const; + + SkeletonModification3DJiggle(); + ~SkeletonModification3DJiggle(); +}; + +#endif //SKELETONMODIFICATION3DJIGGLE_H diff --git a/scene/resources/skeleton_modification_3d_lookat.cpp b/scene/resources/skeleton_modification_3d_lookat.cpp new file mode 100644 index 0000000000..afdb077e71 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_lookat.cpp @@ -0,0 +1,265 @@ +/*************************************************************************/ +/* skeleton_modification_3d_lookat.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/resources/skeleton_modification_3d_lookat.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +bool SkeletonModification3DLookAt::_set(const StringName &p_path, const Variant &p_value) { + if (p_path == "lock_rotation_to_plane") { + set_lock_rotation_to_plane(p_value); + } else if (p_path == "lock_rotation_plane") { + set_lock_rotation_plane(p_value); + } else if (p_path == "additional_rotation") { + Vector3 tmp = p_value; + tmp.x = Math::deg2rad(tmp.x); + tmp.y = Math::deg2rad(tmp.y); + tmp.z = Math::deg2rad(tmp.z); + set_additional_rotation(tmp); + } + + return true; +} + +bool SkeletonModification3DLookAt::_get(const StringName &p_path, Variant &r_ret) const { + if (p_path == "lock_rotation_to_plane") { + r_ret = get_lock_rotation_to_plane(); + } else if (p_path == "lock_rotation_plane") { + r_ret = get_lock_rotation_plane(); + } else if (p_path == "additional_rotation") { + Vector3 tmp = get_additional_rotation(); + tmp.x = Math::rad2deg(tmp.x); + tmp.y = Math::rad2deg(tmp.y); + tmp.z = Math::rad2deg(tmp.z); + r_ret = tmp; + } + + return true; +} + +void SkeletonModification3DLookAt::_get_property_list(List<PropertyInfo> *p_list) const { + p_list->push_back(PropertyInfo(Variant::BOOL, "lock_rotation_to_plane", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + if (lock_rotation_to_plane) { + p_list->push_back(PropertyInfo(Variant::INT, "lock_rotation_plane", PROPERTY_HINT_ENUM, "X plane, Y plane, Z plane", PROPERTY_USAGE_DEFAULT)); + } + p_list->push_back(PropertyInfo(Variant::VECTOR3, "additional_rotation", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); +} + +void SkeletonModification3DLookAt::_execute(real_t p_delta) { + ERR_FAIL_COND_MSG(!stack || !is_setup || stack->skeleton == nullptr, + "Modification is not setup and therefore cannot execute!"); + if (!enabled) { + return; + } + + if (target_node_cache.is_null()) { + _print_execution_error(true, "Target cache is out of date. Attempting to update..."); + update_cache(); + return; + } + + if (bone_idx <= -2) { + bone_idx = stack->skeleton->find_bone(bone_name); + } + + Node3D *target = Object::cast_to<Node3D>(ObjectDB::get_instance(target_node_cache)); + if (_print_execution_error(!target || !target->is_inside_tree(), "Target node is not in the scene tree. Cannot execute modification!")) { + return; + } + if (_print_execution_error(bone_idx <= -1, "Bone index is invalid. Cannot execute modification!")) { + return; + } + + Transform3D new_bone_trans = stack->skeleton->get_bone_local_pose_override(bone_idx); + Vector3 target_pos = stack->skeleton->global_pose_to_local_pose(bone_idx, stack->skeleton->world_transform_to_global_pose(target->get_global_transform())).origin; + + // Lock the rotation to a plane relative to the bone by changing the target position + if (lock_rotation_to_plane) { + if (lock_rotation_plane == ROTATION_PLANE::ROTATION_PLANE_X) { + target_pos.x = new_bone_trans.origin.x; + } else if (lock_rotation_plane == ROTATION_PLANE::ROTATION_PLANE_Y) { + target_pos.y = new_bone_trans.origin.y; + } else if (lock_rotation_plane == ROTATION_PLANE::ROTATION_PLANE_Z) { + target_pos.z = new_bone_trans.origin.z; + } + } + + // Look at the target! + new_bone_trans = new_bone_trans.looking_at(target_pos, Vector3(0, 1, 0)); + // Convert from Z-forward to whatever direction the bone faces. + stack->skeleton->update_bone_rest_forward_vector(bone_idx); + new_bone_trans.basis = stack->skeleton->global_pose_z_forward_to_bone_forward(bone_idx, new_bone_trans.basis); + + // Apply additional rotation + new_bone_trans.basis.rotate_local(Vector3(1, 0, 0), additional_rotation.x); + new_bone_trans.basis.rotate_local(Vector3(0, 1, 0), additional_rotation.y); + new_bone_trans.basis.rotate_local(Vector3(0, 0, 1), additional_rotation.z); + + stack->skeleton->set_bone_local_pose_override(bone_idx, new_bone_trans, stack->strength, true); + stack->skeleton->force_update_bone_children_transforms(bone_idx); + + // If we completed it successfully, then we can set execution_error_found to false + execution_error_found = false; +} + +void SkeletonModification3DLookAt::_setup_modification(SkeletonModificationStack3D *p_stack) { + stack = p_stack; + + if (stack != nullptr) { + is_setup = true; + execution_error_found = false; + update_cache(); + } +} + +void SkeletonModification3DLookAt::set_bone_name(String p_name) { + bone_name = p_name; + if (stack) { + if (stack->skeleton) { + bone_idx = stack->skeleton->find_bone(bone_name); + } + } + execution_error_found = false; + notify_property_list_changed(); +} + +String SkeletonModification3DLookAt::get_bone_name() const { + return bone_name; +} + +int SkeletonModification3DLookAt::get_bone_index() const { + return bone_idx; +} + +void SkeletonModification3DLookAt::set_bone_index(int p_bone_idx) { + ERR_FAIL_COND_MSG(p_bone_idx < 0, "Bone index is out of range: The index is too low!"); + bone_idx = p_bone_idx; + + if (stack) { + if (stack->skeleton) { + bone_name = stack->skeleton->get_bone_name(p_bone_idx); + } + } + execution_error_found = false; + notify_property_list_changed(); +} + +void SkeletonModification3DLookAt::update_cache() { + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update target cache: modification is not properly setup!"); + return; + } + + target_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree()) { + if (stack->skeleton->has_node(target_node)) { + Node *node = stack->skeleton->get_node(target_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update target cache: Node is this modification's skeleton or cannot be found!"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update target cache: Node is not in the scene tree!"); + target_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DLookAt::set_target_node(const NodePath &p_target_node) { + target_node = p_target_node; + update_cache(); +} + +NodePath SkeletonModification3DLookAt::get_target_node() const { + return target_node; +} + +Vector3 SkeletonModification3DLookAt::get_additional_rotation() const { + return additional_rotation; +} + +void SkeletonModification3DLookAt::set_additional_rotation(Vector3 p_offset) { + additional_rotation = p_offset; +} + +bool SkeletonModification3DLookAt::get_lock_rotation_to_plane() const { + return lock_rotation_plane; +} + +void SkeletonModification3DLookAt::set_lock_rotation_to_plane(bool p_lock_rotation) { + lock_rotation_to_plane = p_lock_rotation; + notify_property_list_changed(); +} + +int SkeletonModification3DLookAt::get_lock_rotation_plane() const { + return lock_rotation_plane; +} + +void SkeletonModification3DLookAt::set_lock_rotation_plane(int p_plane) { + lock_rotation_plane = p_plane; +} + +void SkeletonModification3DLookAt::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_bone_name", "name"), &SkeletonModification3DLookAt::set_bone_name); + ClassDB::bind_method(D_METHOD("get_bone_name"), &SkeletonModification3DLookAt::get_bone_name); + + ClassDB::bind_method(D_METHOD("set_bone_index", "bone_idx"), &SkeletonModification3DLookAt::set_bone_index); + ClassDB::bind_method(D_METHOD("get_bone_index"), &SkeletonModification3DLookAt::get_bone_index); + + ClassDB::bind_method(D_METHOD("set_target_node", "target_nodepath"), &SkeletonModification3DLookAt::set_target_node); + ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonModification3DLookAt::get_target_node); + + ClassDB::bind_method(D_METHOD("set_additional_rotation", "additional_rotation"), &SkeletonModification3DLookAt::set_additional_rotation); + ClassDB::bind_method(D_METHOD("get_additional_rotation"), &SkeletonModification3DLookAt::get_additional_rotation); + + ClassDB::bind_method(D_METHOD("set_lock_rotation_to_plane", "lock_to_plane"), &SkeletonModification3DLookAt::set_lock_rotation_to_plane); + ClassDB::bind_method(D_METHOD("get_lock_rotation_to_plane"), &SkeletonModification3DLookAt::get_lock_rotation_to_plane); + ClassDB::bind_method(D_METHOD("set_lock_rotation_plane", "plane"), &SkeletonModification3DLookAt::set_lock_rotation_plane); + ClassDB::bind_method(D_METHOD("get_lock_rotation_plane"), &SkeletonModification3DLookAt::get_lock_rotation_plane); + + ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "bone_name"), "set_bone_name", "get_bone_name"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "bone_index"), "set_bone_index", "get_bone_index"); + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_nodepath", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D"), "set_target_node", "get_target_node"); +} + +SkeletonModification3DLookAt::SkeletonModification3DLookAt() { + stack = nullptr; + is_setup = false; + bone_name = ""; + bone_idx = -2; + additional_rotation = Vector3(); + lock_rotation_to_plane = false; + enabled = true; +} + +SkeletonModification3DLookAt::~SkeletonModification3DLookAt() { +} diff --git a/scene/resources/skeleton_modification_3d_lookat.h b/scene/resources/skeleton_modification_3d_lookat.h new file mode 100644 index 0000000000..5971e3f647 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_lookat.h @@ -0,0 +1,89 @@ +/*************************************************************************/ +/* skeleton_modification_3d_lookat.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +#ifndef SKELETONMODIFICATION3DLOOKAT_H +#define SKELETONMODIFICATION3DLOOKAT_H + +class SkeletonModification3DLookAt : public SkeletonModification3D { + GDCLASS(SkeletonModification3DLookAt, SkeletonModification3D); + +private: + String bone_name = ""; + int bone_idx = -1; + NodePath target_node; + ObjectID target_node_cache; + + Vector3 additional_rotation = Vector3(1, 0, 0); + bool lock_rotation_to_plane = false; + int lock_rotation_plane = ROTATION_PLANE_X; + + void update_cache(); + +protected: + static void _bind_methods(); + bool _get(const StringName &p_path, Variant &r_ret) const; + bool _set(const StringName &p_path, const Variant &p_value); + void _get_property_list(List<PropertyInfo> *p_list) const; + +public: + enum ROTATION_PLANE { + ROTATION_PLANE_X, + ROTATION_PLANE_Y, + ROTATION_PLANE_Z + }; + + virtual void _execute(real_t p_delta) override; + virtual void _setup_modification(SkeletonModificationStack3D *p_stack) override; + + void set_bone_name(String p_name); + String get_bone_name() const; + + void set_bone_index(int p_idx); + int get_bone_index() const; + + void set_target_node(const NodePath &p_target_node); + NodePath get_target_node() const; + + void set_additional_rotation(Vector3 p_offset); + Vector3 get_additional_rotation() const; + + void set_lock_rotation_to_plane(bool p_lock_to_plane); + bool get_lock_rotation_to_plane() const; + void set_lock_rotation_plane(int p_plane); + int get_lock_rotation_plane() const; + + SkeletonModification3DLookAt(); + ~SkeletonModification3DLookAt(); +}; + +#endif //SKELETONMODIFICATION3DLOOKAT_H diff --git a/scene/resources/skeleton_modification_3d_stackholder.cpp b/scene/resources/skeleton_modification_3d_stackholder.cpp new file mode 100644 index 0000000000..56035a4def --- /dev/null +++ b/scene/resources/skeleton_modification_3d_stackholder.cpp @@ -0,0 +1,104 @@ +/*************************************************************************/ +/* skeleton_modification_3d_stackholder.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/resources/skeleton_modification_3d_stackholder.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +bool SkeletonModification3DStackHolder::_set(const StringName &p_path, const Variant &p_value) { + String path = p_path; + + if (path == "held_modification_stack") { + set_held_modification_stack(p_value); + } + return true; +} + +bool SkeletonModification3DStackHolder::_get(const StringName &p_path, Variant &r_ret) const { + String path = p_path; + + if (path == "held_modification_stack") { + r_ret = get_held_modification_stack(); + } + return true; +} + +void SkeletonModification3DStackHolder::_get_property_list(List<PropertyInfo> *p_list) const { + p_list->push_back(PropertyInfo(Variant::OBJECT, "held_modification_stack", PROPERTY_HINT_RESOURCE_TYPE, "SkeletonModificationStack3D", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_DO_NOT_SHARE_ON_DUPLICATE)); +} + +void SkeletonModification3DStackHolder::_execute(real_t p_delta) { + ERR_FAIL_COND_MSG(!stack || !is_setup || stack->skeleton == nullptr, + "Modification is not setup and therefore cannot execute!"); + + if (held_modification_stack.is_valid()) { + held_modification_stack->execute(p_delta, execution_mode); + } +} + +void SkeletonModification3DStackHolder::_setup_modification(SkeletonModificationStack3D *p_stack) { + stack = p_stack; + + if (stack != nullptr) { + is_setup = true; + + if (held_modification_stack.is_valid()) { + held_modification_stack->set_skeleton(stack->get_skeleton()); + held_modification_stack->setup(); + } + } +} + +void SkeletonModification3DStackHolder::set_held_modification_stack(Ref<SkeletonModificationStack3D> p_held_stack) { + held_modification_stack = p_held_stack; + + if (is_setup && held_modification_stack.is_valid()) { + held_modification_stack->set_skeleton(stack->get_skeleton()); + held_modification_stack->setup(); + } +} + +Ref<SkeletonModificationStack3D> SkeletonModification3DStackHolder::get_held_modification_stack() const { + return held_modification_stack; +} + +void SkeletonModification3DStackHolder::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_held_modification_stack", "held_modification_stack"), &SkeletonModification3DStackHolder::set_held_modification_stack); + ClassDB::bind_method(D_METHOD("get_held_modification_stack"), &SkeletonModification3DStackHolder::get_held_modification_stack); +} + +SkeletonModification3DStackHolder::SkeletonModification3DStackHolder() { + stack = nullptr; + is_setup = false; + enabled = true; +} + +SkeletonModification3DStackHolder::~SkeletonModification3DStackHolder() { +} diff --git a/scene/resources/skeleton_modification_3d_stackholder.h b/scene/resources/skeleton_modification_3d_stackholder.h new file mode 100644 index 0000000000..c765cd8de3 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_stackholder.h @@ -0,0 +1,59 @@ +/*************************************************************************/ +/* skeleton_modification_3d_stackholder.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +#ifndef SKELETONMODIFICATION3DSTACKHOLDER_H +#define SKELETONMODIFICATION3DSTACKHOLDER_H + +class SkeletonModification3DStackHolder : public SkeletonModification3D { + GDCLASS(SkeletonModification3DStackHolder, SkeletonModification3D); + +protected: + static void _bind_methods(); + bool _get(const StringName &p_path, Variant &r_ret) const; + bool _set(const StringName &p_path, const Variant &p_value); + void _get_property_list(List<PropertyInfo> *p_list) const; + +public: + Ref<SkeletonModificationStack3D> held_modification_stack; + + virtual void _execute(real_t p_delta) override; + virtual void _setup_modification(SkeletonModificationStack3D *p_stack) override; + + void set_held_modification_stack(Ref<SkeletonModificationStack3D> p_held_stack); + Ref<SkeletonModificationStack3D> get_held_modification_stack() const; + + SkeletonModification3DStackHolder(); + ~SkeletonModification3DStackHolder(); +}; + +#endif //SKELETONMODIFICATION3DSTACKHOLDER_H diff --git a/scene/resources/skeleton_modification_3d_twoboneik.cpp b/scene/resources/skeleton_modification_3d_twoboneik.cpp new file mode 100644 index 0000000000..ae7a3bab7e --- /dev/null +++ b/scene/resources/skeleton_modification_3d_twoboneik.cpp @@ -0,0 +1,599 @@ +/*************************************************************************/ +/* skeleton_modification_3d_twoboneik.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/resources/skeleton_modification_3d_twoboneik.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +bool SkeletonModification3DTwoBoneIK::_set(const StringName &p_path, const Variant &p_value) { + String path = p_path; + + if (path == "use_tip_node") { + set_use_tip_node(p_value); + } else if (path == "tip_node") { + set_tip_node(p_value); + } else if (path == "auto_calculate_joint_length") { + set_auto_calculate_joint_length(p_value); + } else if (path == "use_pole_node") { + set_use_pole_node(p_value); + } else if (path == "pole_node") { + set_pole_node(p_value); + } else if (path == "joint_one_length") { + set_joint_one_length(p_value); + } else if (path == "joint_two_length") { + set_joint_two_length(p_value); + } else if (path == "joint_one/bone_name") { + set_joint_one_bone_name(p_value); + } else if (path == "joint_one/bone_idx") { + set_joint_one_bone_idx(p_value); + } else if (path == "joint_one/roll") { + set_joint_one_roll(Math::deg2rad(real_t(p_value))); + } else if (path == "joint_two/bone_name") { + set_joint_two_bone_name(p_value); + } else if (path == "joint_two/bone_idx") { + set_joint_two_bone_idx(p_value); + } else if (path == "joint_two/roll") { + set_joint_two_roll(Math::deg2rad(real_t(p_value))); + } + + return true; +} + +bool SkeletonModification3DTwoBoneIK::_get(const StringName &p_path, Variant &r_ret) const { + String path = p_path; + + if (path == "use_tip_node") { + r_ret = get_use_tip_node(); + } else if (path == "tip_node") { + r_ret = get_tip_node(); + } else if (path == "auto_calculate_joint_length") { + r_ret = get_auto_calculate_joint_length(); + } else if (path == "use_pole_node") { + r_ret = get_use_pole_node(); + } else if (path == "pole_node") { + r_ret = get_pole_node(); + } else if (path == "joint_one_length") { + r_ret = get_joint_one_length(); + } else if (path == "joint_two_length") { + r_ret = get_joint_two_length(); + } else if (path == "joint_one/bone_name") { + r_ret = get_joint_one_bone_name(); + } else if (path == "joint_one/bone_idx") { + r_ret = get_joint_one_bone_idx(); + } else if (path == "joint_one/roll") { + r_ret = Math::rad2deg(get_joint_one_roll()); + } else if (path == "joint_two/bone_name") { + r_ret = get_joint_two_bone_name(); + } else if (path == "joint_two/bone_idx") { + r_ret = get_joint_two_bone_idx(); + } else if (path == "joint_two/roll") { + r_ret = Math::rad2deg(get_joint_two_roll()); + } + + return true; +} + +void SkeletonModification3DTwoBoneIK::_get_property_list(List<PropertyInfo> *p_list) const { + p_list->push_back(PropertyInfo(Variant::BOOL, "use_tip_node", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + if (use_tip_node) { + p_list->push_back(PropertyInfo(Variant::NODE_PATH, "tip_node", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D", PROPERTY_USAGE_DEFAULT)); + } + + p_list->push_back(PropertyInfo(Variant::BOOL, "auto_calculate_joint_length", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + if (!auto_calculate_joint_length) { + p_list->push_back(PropertyInfo(Variant::FLOAT, "joint_one_length", PROPERTY_HINT_RANGE, "-1, 10000, 0.001", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::FLOAT, "joint_two_length", PROPERTY_HINT_RANGE, "-1, 10000, 0.001", PROPERTY_USAGE_DEFAULT)); + } + + p_list->push_back(PropertyInfo(Variant::BOOL, "use_pole_node", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + if (use_pole_node) { + p_list->push_back(PropertyInfo(Variant::NODE_PATH, "pole_node", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D", PROPERTY_USAGE_DEFAULT)); + } + + p_list->push_back(PropertyInfo(Variant::STRING_NAME, "joint_one/bone_name", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::INT, "joint_one/bone_idx", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::FLOAT, "joint_one/roll", PROPERTY_HINT_RANGE, "-360, 360, 0.01", PROPERTY_USAGE_DEFAULT)); + + p_list->push_back(PropertyInfo(Variant::STRING_NAME, "joint_two/bone_name", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::INT, "joint_two/bone_idx", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT)); + p_list->push_back(PropertyInfo(Variant::FLOAT, "joint_two/roll", PROPERTY_HINT_RANGE, "-360, 360, 0.01", PROPERTY_USAGE_DEFAULT)); +} + +void SkeletonModification3DTwoBoneIK::_execute(real_t p_delta) { + ERR_FAIL_COND_MSG(!stack || !is_setup || stack->skeleton == nullptr, + "Modification is not setup and therefore cannot execute!"); + + if (!enabled) { + return; + } + + if (_print_execution_error(joint_one_bone_idx < 0 || joint_two_bone_idx < 0, + "One (or more) of the bones in the modification have invalid bone indexes. Cannot execute modification!")) { + return; + } + + if (target_node_cache.is_null()) { + _print_execution_error(true, "Target cache is out of date. Attempting to update..."); + update_cache_target(); + return; + } + + // Update joint lengths (if needed) + if (auto_calculate_joint_length && (joint_one_length < 0 || joint_two_length < 0)) { + calculate_joint_lengths(); + } + + // Adopted from the links below: + // http://theorangeduck.com/page/simple-two-joint + // https://www.alanzucconi.com/2018/05/02/ik-2d-2/ + // With modifications by TwistedTwigleg + Node3D *target = Object::cast_to<Node3D>(ObjectDB::get_instance(target_node_cache)); + if (_print_execution_error(!target || !target->is_inside_tree(), "Target node is not in the scene tree. Cannot execute modification!")) { + return; + } + Transform3D target_trans = stack->skeleton->world_transform_to_global_pose(target->get_global_transform()); + + Transform3D bone_one_trans; + Transform3D bone_two_trans; + + // Make the first joint look at the pole, and the second look at the target. That way, the + // TwoBoneIK solver has to really only handle extension/contraction, which should make it align with the pole. + if (use_pole_node) { + if (pole_node_cache.is_null()) { + _print_execution_error(true, "Pole cache is out of date. Attempting to update..."); + update_cache_pole(); + return; + } + + Node3D *pole = Object::cast_to<Node3D>(ObjectDB::get_instance(pole_node_cache)); + if (_print_execution_error(!pole || !pole->is_inside_tree(), "Pole node is not in the scene tree. Cannot execute modification!")) { + return; + } + Transform3D pole_trans = stack->skeleton->world_transform_to_global_pose(pole->get_global_transform()); + + bone_one_trans = stack->skeleton->local_pose_to_global_pose(joint_one_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_one_bone_idx)); + bone_one_trans = bone_one_trans.looking_at(pole_trans.origin, Vector3(0, 1, 0)); + bone_one_trans.basis = stack->skeleton->global_pose_z_forward_to_bone_forward(joint_one_bone_idx, bone_one_trans.basis); + stack->skeleton->update_bone_rest_forward_vector(joint_one_bone_idx); + bone_one_trans.basis.rotate_local(stack->skeleton->get_bone_axis_forward_vector(joint_one_bone_idx), joint_one_roll); + stack->skeleton->set_bone_local_pose_override(joint_one_bone_idx, stack->skeleton->global_pose_to_local_pose(joint_one_bone_idx, bone_one_trans), stack->strength, true); + stack->skeleton->force_update_bone_children_transforms(joint_one_bone_idx); + + bone_two_trans = stack->skeleton->local_pose_to_global_pose(joint_two_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_two_bone_idx)); + bone_two_trans = bone_two_trans.looking_at(target_trans.origin, Vector3(0, 1, 0)); + bone_two_trans.basis = stack->skeleton->global_pose_z_forward_to_bone_forward(joint_two_bone_idx, bone_two_trans.basis); + stack->skeleton->update_bone_rest_forward_vector(joint_two_bone_idx); + bone_two_trans.basis.rotate_local(stack->skeleton->get_bone_axis_forward_vector(joint_two_bone_idx), joint_two_roll); + stack->skeleton->set_bone_local_pose_override(joint_two_bone_idx, stack->skeleton->global_pose_to_local_pose(joint_two_bone_idx, bone_two_trans), stack->strength, true); + stack->skeleton->force_update_bone_children_transforms(joint_two_bone_idx); + } else { + bone_one_trans = stack->skeleton->local_pose_to_global_pose(joint_one_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_one_bone_idx)); + bone_two_trans = stack->skeleton->local_pose_to_global_pose(joint_two_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_two_bone_idx)); + } + + Transform3D bone_two_tip_trans; + if (use_tip_node) { + if (tip_node_cache.is_null()) { + _print_execution_error(true, "Tip cache is out of date. Attempting to update..."); + update_cache_tip(); + return; + } + Node3D *tip = Object::cast_to<Node3D>(ObjectDB::get_instance(tip_node_cache)); + if (_print_execution_error(!tip || !tip->is_inside_tree(), "Tip node is not in the scene tree. Cannot execute modification!")) { + return; + } + bone_two_tip_trans = stack->skeleton->world_transform_to_global_pose(tip->get_global_transform()); + } else { + stack->skeleton->update_bone_rest_forward_vector(joint_two_bone_idx); + bone_two_tip_trans = bone_two_trans; + bone_two_tip_trans.origin += bone_two_trans.basis.xform(stack->skeleton->get_bone_axis_forward_vector(joint_two_bone_idx)).normalized() * joint_two_length; + } + + real_t joint_one_to_target_length = bone_one_trans.origin.distance_to(target_trans.origin); + if (joint_one_length + joint_two_length < joint_one_to_target_length) { + // Set the target *just* out of reach to straighten the bones + joint_one_to_target_length = joint_one_length + joint_two_length + 0.01; + } else if (joint_one_to_target_length < joint_one_length) { + // Place the target in reach so the solver doesn't do crazy things + joint_one_to_target_length = joint_one_length; + } + + // Get the square lengths for all three sides of the triangle we'll use to calculate the angles + real_t sqr_one_length = joint_one_length * joint_one_length; + real_t sqr_two_length = joint_two_length * joint_two_length; + real_t sqr_three_length = joint_one_to_target_length * joint_one_to_target_length; + + // Calculate the angles for the first joint using the law of cosigns + real_t ac_ab_0 = Math::acos(CLAMP(bone_two_tip_trans.origin.direction_to(bone_one_trans.origin).dot(bone_two_trans.origin.direction_to(bone_one_trans.origin)), -1, 1)); + real_t ac_at_0 = Math::acos(CLAMP(bone_one_trans.origin.direction_to(bone_two_tip_trans.origin).dot(bone_one_trans.origin.direction_to(target_trans.origin)), -1, 1)); + real_t ac_ab_1 = Math::acos(CLAMP((sqr_two_length - sqr_one_length - sqr_three_length) / (-2.0 * joint_one_length * joint_one_to_target_length), -1, 1)); + + // Calculate the angles of rotation. Angle 0 is the extension/contraction axis, while angle 1 is the rotation axis to align the triangle to the target + Vector3 axis_0 = bone_one_trans.origin.direction_to(bone_two_tip_trans.origin).cross(bone_one_trans.origin.direction_to(bone_two_trans.origin)); + Vector3 axis_1 = bone_one_trans.origin.direction_to(bone_two_tip_trans.origin).cross(bone_one_trans.origin.direction_to(target_trans.origin)); + + // Make a quaternion with the delta rotation needed to rotate the first joint into alignment and apply it to the transform. + Quaternion bone_one_quat = bone_one_trans.basis.get_rotation_quaternion(); + Quaternion rot_0 = Quaternion(bone_one_quat.inverse().xform(axis_0).normalized(), (ac_ab_1 - ac_ab_0)); + Quaternion rot_2 = Quaternion(bone_one_quat.inverse().xform(axis_1).normalized(), ac_at_0); + bone_one_trans.basis.set_quaternion(bone_one_quat * (rot_0 * rot_2)); + + stack->skeleton->update_bone_rest_forward_vector(joint_one_bone_idx); + bone_one_trans.basis.rotate_local(stack->skeleton->get_bone_axis_forward_vector(joint_one_bone_idx), joint_one_roll); + + // Apply the rotation to the first joint + bone_one_trans = stack->skeleton->global_pose_to_local_pose(joint_one_bone_idx, bone_one_trans); + bone_one_trans.origin = Vector3(0, 0, 0); + stack->skeleton->set_bone_local_pose_override(joint_one_bone_idx, bone_one_trans, stack->strength, true); + stack->skeleton->force_update_bone_children_transforms(joint_one_bone_idx); + + if (use_pole_node) { + // Update bone_two_trans so its at the latest position, with the rotation of bone_one_trans taken into account, then look at the target. + bone_two_trans = stack->skeleton->local_pose_to_global_pose(joint_two_bone_idx, stack->skeleton->get_bone_local_pose_override(joint_two_bone_idx)); + stack->skeleton->update_bone_rest_forward_vector(joint_two_bone_idx); + Vector3 forward_vector = stack->skeleton->get_bone_axis_forward_vector(joint_two_bone_idx); + bone_two_trans.basis.rotate_to_align(forward_vector, bone_two_trans.origin.direction_to(target_trans.origin)); + + stack->skeleton->update_bone_rest_forward_vector(joint_two_bone_idx); + bone_two_trans.basis.rotate_local(stack->skeleton->get_bone_axis_forward_vector(joint_two_bone_idx), joint_two_roll); + + bone_two_trans = stack->skeleton->global_pose_to_local_pose(joint_two_bone_idx, bone_two_trans); + stack->skeleton->set_bone_local_pose_override(joint_two_bone_idx, bone_two_trans, stack->strength, true); + stack->skeleton->force_update_bone_children_transforms(joint_two_bone_idx); + } else { + // Calculate the angles for the second joint using the law of cosigns, make a quaternion with the delta rotation needed to rotate the joint into + // alignment, and then apply it to the second joint. + real_t ba_bc_0 = Math::acos(CLAMP(bone_two_trans.origin.direction_to(bone_one_trans.origin).dot(bone_two_trans.origin.direction_to(bone_two_tip_trans.origin)), -1, 1)); + real_t ba_bc_1 = Math::acos(CLAMP((sqr_three_length - sqr_one_length - sqr_two_length) / (-2.0 * joint_one_length * joint_two_length), -1, 1)); + Quaternion bone_two_quat = bone_two_trans.basis.get_rotation_quaternion(); + Quaternion rot_1 = Quaternion(bone_two_quat.inverse().xform(axis_0).normalized(), (ba_bc_1 - ba_bc_0)); + bone_two_trans.basis.set_quaternion(bone_two_quat * rot_1); + + stack->skeleton->update_bone_rest_forward_vector(joint_two_bone_idx); + bone_two_trans.basis.rotate_local(stack->skeleton->get_bone_axis_forward_vector(joint_two_bone_idx), joint_two_roll); + + bone_two_trans = stack->skeleton->global_pose_to_local_pose(joint_two_bone_idx, bone_two_trans); + bone_two_trans.origin = Vector3(0, 0, 0); + stack->skeleton->set_bone_local_pose_override(joint_two_bone_idx, bone_two_trans, stack->strength, true); + stack->skeleton->force_update_bone_children_transforms(joint_two_bone_idx); + } +} + +void SkeletonModification3DTwoBoneIK::_setup_modification(SkeletonModificationStack3D *p_stack) { + stack = p_stack; + + if (stack != nullptr) { + is_setup = true; + execution_error_found = false; + update_cache_target(); + update_cache_tip(); + } +} + +void SkeletonModification3DTwoBoneIK::update_cache_target() { + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update target cache: modification is not properly setup!"); + return; + } + + target_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree() && target_node.is_empty() == false) { + if (stack->skeleton->has_node(target_node)) { + Node *node = stack->skeleton->get_node(target_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update target cache: Target node is this modification's skeleton or cannot be found. Cannot execute modification"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update target cache: Target node is not in the scene tree. Cannot execute modification!"); + target_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DTwoBoneIK::update_cache_tip() { + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update tip cache: modification is not properly setup!"); + return; + } + + tip_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree()) { + if (stack->skeleton->has_node(tip_node)) { + Node *node = stack->skeleton->get_node(tip_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update tip cache: Tip node is this modification's skeleton or cannot be found!"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update tip cache: Tip node is not in the scene tree. Cannot execute modification!"); + tip_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DTwoBoneIK::update_cache_pole() { + if (!is_setup || !stack) { + _print_execution_error(true, "Cannot update pole cache: modification is not properly setup!"); + return; + } + + pole_node_cache = ObjectID(); + if (stack->skeleton) { + if (stack->skeleton->is_inside_tree()) { + if (stack->skeleton->has_node(pole_node)) { + Node *node = stack->skeleton->get_node(pole_node); + ERR_FAIL_COND_MSG(!node || stack->skeleton == node, + "Cannot update pole cache: Pole node is this modification's skeleton or cannot be found!"); + ERR_FAIL_COND_MSG(!node->is_inside_tree(), + "Cannot update pole cache: Pole node is not in the scene tree. Cannot execute modification!"); + pole_node_cache = node->get_instance_id(); + + execution_error_found = false; + } + } + } +} + +void SkeletonModification3DTwoBoneIK::set_target_node(const NodePath &p_target_node) { + target_node = p_target_node; + update_cache_target(); +} + +NodePath SkeletonModification3DTwoBoneIK::get_target_node() const { + return target_node; +} + +void SkeletonModification3DTwoBoneIK::set_use_tip_node(const bool p_use_tip_node) { + use_tip_node = p_use_tip_node; + notify_property_list_changed(); +} + +bool SkeletonModification3DTwoBoneIK::get_use_tip_node() const { + return use_tip_node; +} + +void SkeletonModification3DTwoBoneIK::set_tip_node(const NodePath &p_tip_node) { + tip_node = p_tip_node; + update_cache_tip(); +} + +NodePath SkeletonModification3DTwoBoneIK::get_tip_node() const { + return tip_node; +} + +void SkeletonModification3DTwoBoneIK::set_use_pole_node(const bool p_use_pole_node) { + use_pole_node = p_use_pole_node; + notify_property_list_changed(); +} + +bool SkeletonModification3DTwoBoneIK::get_use_pole_node() const { + return use_pole_node; +} + +void SkeletonModification3DTwoBoneIK::set_pole_node(const NodePath &p_pole_node) { + pole_node = p_pole_node; + update_cache_pole(); +} + +NodePath SkeletonModification3DTwoBoneIK::get_pole_node() const { + return pole_node; +} + +void SkeletonModification3DTwoBoneIK::set_auto_calculate_joint_length(bool p_calculate) { + auto_calculate_joint_length = p_calculate; + if (p_calculate) { + calculate_joint_lengths(); + } + notify_property_list_changed(); +} + +bool SkeletonModification3DTwoBoneIK::get_auto_calculate_joint_length() const { + return auto_calculate_joint_length; +} + +void SkeletonModification3DTwoBoneIK::calculate_joint_lengths() { + if (!is_setup) { + return; // fail silently, as we likely just loaded the scene. + } + ERR_FAIL_COND_MSG(!stack || stack->skeleton == nullptr, + "Modification is not setup and therefore cannot calculate joint lengths!"); + ERR_FAIL_COND_MSG(joint_one_bone_idx <= -1 || joint_two_bone_idx <= -1, + "One of the bones in the TwoBoneIK modification are not set! Cannot calculate joint lengths!"); + + Transform3D bone_one_rest_trans = stack->skeleton->get_bone_global_pose(joint_one_bone_idx); + Transform3D bone_two_rest_trans = stack->skeleton->get_bone_global_pose(joint_two_bone_idx); + + joint_one_length = bone_one_rest_trans.origin.distance_to(bone_two_rest_trans.origin); + + if (use_tip_node) { + if (tip_node_cache.is_null()) { + update_cache_tip(); + WARN_PRINT("Tip cache is out of date. Updating..."); + } + + Node3D *tip = Object::cast_to<Node3D>(ObjectDB::get_instance(tip_node_cache)); + if (tip) { + Transform3D bone_tip_trans = stack->skeleton->world_transform_to_global_pose(tip->get_global_transform()); + joint_two_length = bone_two_rest_trans.origin.distance_to(bone_tip_trans.origin); + } + } else { + // Attempt to use children bones to get the length + Vector<int> bone_two_children = stack->skeleton->get_bone_children(joint_two_bone_idx); + if (bone_two_children.size() > 0) { + joint_two_length = 0; + for (int i = 0; i < bone_two_children.size(); i++) { + joint_two_length += bone_two_rest_trans.origin.distance_to( + stack->skeleton->local_pose_to_global_pose(bone_two_children[i], stack->skeleton->get_bone_rest(bone_two_children[i])).origin); + } + joint_two_length = joint_two_length / bone_two_children.size(); + } else { + WARN_PRINT("TwoBoneIK modification: Cannot auto calculate length for joint 2! Auto setting the length to 1..."); + joint_two_length = 1.0; + } + } + execution_error_found = false; +} + +void SkeletonModification3DTwoBoneIK::set_joint_one_bone_name(String p_bone_name) { + joint_one_bone_name = p_bone_name; + if (stack && stack->skeleton) { + joint_one_bone_idx = stack->skeleton->find_bone(p_bone_name); + } + execution_error_found = false; + notify_property_list_changed(); +} + +String SkeletonModification3DTwoBoneIK::get_joint_one_bone_name() const { + return joint_one_bone_name; +} + +void SkeletonModification3DTwoBoneIK::set_joint_one_bone_idx(int p_bone_idx) { + joint_one_bone_idx = p_bone_idx; + if (stack && stack->skeleton) { + joint_one_bone_name = stack->skeleton->get_bone_name(p_bone_idx); + } + execution_error_found = false; + notify_property_list_changed(); +} + +int SkeletonModification3DTwoBoneIK::get_joint_one_bone_idx() const { + return joint_one_bone_idx; +} + +void SkeletonModification3DTwoBoneIK::set_joint_one_length(real_t p_length) { + joint_one_length = p_length; +} + +real_t SkeletonModification3DTwoBoneIK::get_joint_one_length() const { + return joint_one_length; +} + +void SkeletonModification3DTwoBoneIK::set_joint_two_bone_name(String p_bone_name) { + joint_two_bone_name = p_bone_name; + if (stack && stack->skeleton) { + joint_two_bone_idx = stack->skeleton->find_bone(p_bone_name); + } + execution_error_found = false; + notify_property_list_changed(); +} + +String SkeletonModification3DTwoBoneIK::get_joint_two_bone_name() const { + return joint_two_bone_name; +} + +void SkeletonModification3DTwoBoneIK::set_joint_two_bone_idx(int p_bone_idx) { + joint_two_bone_idx = p_bone_idx; + if (stack && stack->skeleton) { + joint_two_bone_name = stack->skeleton->get_bone_name(p_bone_idx); + } + execution_error_found = false; + notify_property_list_changed(); +} + +int SkeletonModification3DTwoBoneIK::get_joint_two_bone_idx() const { + return joint_two_bone_idx; +} + +void SkeletonModification3DTwoBoneIK::set_joint_two_length(real_t p_length) { + joint_two_length = p_length; +} + +real_t SkeletonModification3DTwoBoneIK::get_joint_two_length() const { + return joint_two_length; +} + +void SkeletonModification3DTwoBoneIK::set_joint_one_roll(real_t p_roll) { + joint_one_roll = p_roll; +} + +real_t SkeletonModification3DTwoBoneIK::get_joint_one_roll() const { + return joint_one_roll; +} + +void SkeletonModification3DTwoBoneIK::set_joint_two_roll(real_t p_roll) { + joint_two_roll = p_roll; +} + +real_t SkeletonModification3DTwoBoneIK::get_joint_two_roll() const { + return joint_two_roll; +} + +void SkeletonModification3DTwoBoneIK::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_target_node", "target_nodepath"), &SkeletonModification3DTwoBoneIK::set_target_node); + ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonModification3DTwoBoneIK::get_target_node); + + ClassDB::bind_method(D_METHOD("set_use_pole_node", "use_pole_node"), &SkeletonModification3DTwoBoneIK::set_use_pole_node); + ClassDB::bind_method(D_METHOD("get_use_pole_node"), &SkeletonModification3DTwoBoneIK::get_use_pole_node); + ClassDB::bind_method(D_METHOD("set_pole_node", "pole_nodepath"), &SkeletonModification3DTwoBoneIK::set_pole_node); + ClassDB::bind_method(D_METHOD("get_pole_node"), &SkeletonModification3DTwoBoneIK::get_pole_node); + + ClassDB::bind_method(D_METHOD("set_use_tip_node", "use_tip_node"), &SkeletonModification3DTwoBoneIK::set_use_tip_node); + ClassDB::bind_method(D_METHOD("get_use_tip_node"), &SkeletonModification3DTwoBoneIK::get_use_tip_node); + ClassDB::bind_method(D_METHOD("set_tip_node", "tip_nodepath"), &SkeletonModification3DTwoBoneIK::set_tip_node); + ClassDB::bind_method(D_METHOD("get_tip_node"), &SkeletonModification3DTwoBoneIK::get_tip_node); + + ClassDB::bind_method(D_METHOD("set_auto_calculate_joint_length", "auto_calculate_joint_length"), &SkeletonModification3DTwoBoneIK::set_auto_calculate_joint_length); + ClassDB::bind_method(D_METHOD("get_auto_calculate_joint_length"), &SkeletonModification3DTwoBoneIK::get_auto_calculate_joint_length); + + ClassDB::bind_method(D_METHOD("set_joint_one_bone_name", "bone_name"), &SkeletonModification3DTwoBoneIK::set_joint_one_bone_name); + ClassDB::bind_method(D_METHOD("get_joint_one_bone_name"), &SkeletonModification3DTwoBoneIK::get_joint_one_bone_name); + ClassDB::bind_method(D_METHOD("set_joint_one_bone_idx", "bone_idx"), &SkeletonModification3DTwoBoneIK::set_joint_one_bone_idx); + ClassDB::bind_method(D_METHOD("get_joint_one_bone_idx"), &SkeletonModification3DTwoBoneIK::get_joint_one_bone_idx); + ClassDB::bind_method(D_METHOD("set_joint_one_length", "bone_length"), &SkeletonModification3DTwoBoneIK::set_joint_one_length); + ClassDB::bind_method(D_METHOD("get_joint_one_length"), &SkeletonModification3DTwoBoneIK::get_joint_one_length); + + ClassDB::bind_method(D_METHOD("set_joint_two_bone_name", "bone_name"), &SkeletonModification3DTwoBoneIK::set_joint_two_bone_name); + ClassDB::bind_method(D_METHOD("get_joint_two_bone_name"), &SkeletonModification3DTwoBoneIK::get_joint_two_bone_name); + ClassDB::bind_method(D_METHOD("set_joint_two_bone_idx", "bone_idx"), &SkeletonModification3DTwoBoneIK::set_joint_two_bone_idx); + ClassDB::bind_method(D_METHOD("get_joint_two_bone_idx"), &SkeletonModification3DTwoBoneIK::get_joint_two_bone_idx); + ClassDB::bind_method(D_METHOD("set_joint_two_length", "bone_length"), &SkeletonModification3DTwoBoneIK::set_joint_two_length); + ClassDB::bind_method(D_METHOD("get_joint_two_length"), &SkeletonModification3DTwoBoneIK::get_joint_two_length); + + ClassDB::bind_method(D_METHOD("set_joint_one_roll", "roll"), &SkeletonModification3DTwoBoneIK::set_joint_one_roll); + ClassDB::bind_method(D_METHOD("get_joint_one_roll"), &SkeletonModification3DTwoBoneIK::get_joint_one_roll); + ClassDB::bind_method(D_METHOD("set_joint_two_roll", "roll"), &SkeletonModification3DTwoBoneIK::set_joint_two_roll); + ClassDB::bind_method(D_METHOD("get_joint_two_roll"), &SkeletonModification3DTwoBoneIK::get_joint_two_roll); + + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_nodepath", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D"), "set_target_node", "get_target_node"); + ADD_GROUP("", ""); +} + +SkeletonModification3DTwoBoneIK::SkeletonModification3DTwoBoneIK() { + stack = nullptr; + is_setup = false; +} + +SkeletonModification3DTwoBoneIK::~SkeletonModification3DTwoBoneIK() { +} diff --git a/scene/resources/skeleton_modification_3d_twoboneik.h b/scene/resources/skeleton_modification_3d_twoboneik.h new file mode 100644 index 0000000000..e62d6cc497 --- /dev/null +++ b/scene/resources/skeleton_modification_3d_twoboneik.h @@ -0,0 +1,118 @@ +/*************************************************************************/ +/* skeleton_modification_3d_twoboneik.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "scene/3d/skeleton_3d.h" +#include "scene/resources/skeleton_modification_3d.h" + +#ifndef SKELETONMODIFICATION3DTWOBONEIK_H +#define SKELETONMODIFICATION3DTWOBONEIK_H + +class SkeletonModification3DTwoBoneIK : public SkeletonModification3D { + GDCLASS(SkeletonModification3DTwoBoneIK, SkeletonModification3D); + +private: + NodePath target_node; + ObjectID target_node_cache; + + bool use_tip_node = false; + NodePath tip_node; + ObjectID tip_node_cache; + + bool use_pole_node = false; + NodePath pole_node; + ObjectID pole_node_cache; + + String joint_one_bone_name = ""; + int joint_one_bone_idx = -1; + String joint_two_bone_name = ""; + int joint_two_bone_idx = -1; + + bool auto_calculate_joint_length = false; + real_t joint_one_length = -1; + real_t joint_two_length = -1; + + real_t joint_one_roll = 0; + real_t joint_two_roll = 0; + + void update_cache_target(); + void update_cache_tip(); + void update_cache_pole(); + +protected: + static void _bind_methods(); + bool _get(const StringName &p_path, Variant &r_ret) const; + bool _set(const StringName &p_path, const Variant &p_value); + void _get_property_list(List<PropertyInfo> *p_list) const; + +public: + virtual void _execute(real_t p_delta) override; + virtual void _setup_modification(SkeletonModificationStack3D *p_stack) override; + + void set_target_node(const NodePath &p_target_node); + NodePath get_target_node() const; + + void set_use_tip_node(const bool p_use_tip_node); + bool get_use_tip_node() const; + void set_tip_node(const NodePath &p_tip_node); + NodePath get_tip_node() const; + + void set_use_pole_node(const bool p_use_pole_node); + bool get_use_pole_node() const; + void set_pole_node(const NodePath &p_pole_node); + NodePath get_pole_node() const; + + void set_auto_calculate_joint_length(bool p_calculate); + bool get_auto_calculate_joint_length() const; + void calculate_joint_lengths(); + + void set_joint_one_bone_name(String p_bone_name); + String get_joint_one_bone_name() const; + void set_joint_one_bone_idx(int p_bone_idx); + int get_joint_one_bone_idx() const; + void set_joint_one_length(real_t p_length); + real_t get_joint_one_length() const; + + void set_joint_two_bone_name(String p_bone_name); + String get_joint_two_bone_name() const; + void set_joint_two_bone_idx(int p_bone_idx); + int get_joint_two_bone_idx() const; + void set_joint_two_length(real_t p_length); + real_t get_joint_two_length() const; + + void set_joint_one_roll(real_t p_roll); + real_t get_joint_one_roll() const; + void set_joint_two_roll(real_t p_roll); + real_t get_joint_two_roll() const; + + SkeletonModification3DTwoBoneIK(); + ~SkeletonModification3DTwoBoneIK(); +}; + +#endif //SKELETONMODIFICATION3DTWOBONEIK_H diff --git a/scene/resources/skeleton_modification_stack_3d.cpp b/scene/resources/skeleton_modification_stack_3d.cpp new file mode 100644 index 0000000000..3fce0e5dbd --- /dev/null +++ b/scene/resources/skeleton_modification_stack_3d.cpp @@ -0,0 +1,222 @@ +/*************************************************************************/ +/* skeleton_modification_stack_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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_modification_stack_3d.h" +#include "scene/3d/skeleton_3d.h" + +/////////////////////////////////////// +// ModificationStack3D +/////////////////////////////////////// + +void SkeletonModificationStack3D::_get_property_list(List<PropertyInfo> *p_list) const { + for (uint32_t i = 0; i < modifications.size(); i++) { + p_list->push_back( + PropertyInfo(Variant::OBJECT, "modifications/" + itos(i), + PROPERTY_HINT_RESOURCE_TYPE, + "SkeletonModification3D", + PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_DEFERRED_SET_RESOURCE | PROPERTY_USAGE_DO_NOT_SHARE_ON_DUPLICATE)); + } +} + +bool SkeletonModificationStack3D::_set(const StringName &p_path, const Variant &p_value) { + String path = p_path; + + if (path.begins_with("modifications/")) { + int mod_idx = path.get_slicec('/', 1).to_int(); + set_modification(mod_idx, p_value); + return true; + } + return true; +} + +bool SkeletonModificationStack3D::_get(const StringName &p_path, Variant &r_ret) const { + String path = p_path; + + if (path.begins_with("modifications/")) { + int mod_idx = path.get_slicec('/', 1).to_int(); + r_ret = get_modification(mod_idx); + return true; + } + return true; +} + +void SkeletonModificationStack3D::setup() { + if (is_setup) { + return; + } + + if (skeleton != nullptr) { + is_setup = true; + for (uint32_t i = 0; i < modifications.size(); i++) { + if (!modifications[i].is_valid()) { + continue; + } + modifications[i]->_setup_modification(this); + } + } else { + WARN_PRINT("Cannot setup SkeletonModificationStack3D: no skeleton set!"); + } +} + +void SkeletonModificationStack3D::execute(real_t p_delta, int p_execution_mode) { + ERR_FAIL_COND_MSG(!is_setup || skeleton == nullptr || is_queued_for_deletion(), + "Modification stack is not properly setup and therefore cannot execute!"); + + if (!skeleton->is_inside_tree()) { + ERR_PRINT_ONCE("Skeleton is not inside SceneTree! Cannot execute modification!"); + return; + } + + if (!enabled) { + return; + } + + for (uint32_t i = 0; i < modifications.size(); i++) { + if (!modifications[i].is_valid()) { + continue; + } + + if (modifications[i]->get_execution_mode() == p_execution_mode) { + modifications[i]->_execute(p_delta); + } + } +} + +void SkeletonModificationStack3D::enable_all_modifications(bool p_enabled) { + for (uint32_t i = 0; i < modifications.size(); i++) { + if (!modifications[i].is_valid()) { + continue; + } + modifications[i]->set_enabled(p_enabled); + } +} + +Ref<SkeletonModification3D> SkeletonModificationStack3D::get_modification(int p_mod_idx) const { + const int modifications_size = modifications.size(); + ERR_FAIL_INDEX_V(p_mod_idx, modifications_size, nullptr); + return modifications[p_mod_idx]; +} + +void SkeletonModificationStack3D::add_modification(Ref<SkeletonModification3D> p_mod) { + p_mod->_setup_modification(this); + modifications.push_back(p_mod); +} + +void SkeletonModificationStack3D::delete_modification(int p_mod_idx) { + const int modifications_size = modifications.size(); + ERR_FAIL_INDEX(p_mod_idx, modifications_size); + modifications.remove(p_mod_idx); +} + +void SkeletonModificationStack3D::set_modification(int p_mod_idx, Ref<SkeletonModification3D> p_mod) { + const int modifications_size = modifications.size(); + ERR_FAIL_INDEX(p_mod_idx, modifications_size); + + if (p_mod == nullptr) { + modifications.remove(p_mod_idx); + } else { + p_mod->_setup_modification(this); + modifications[p_mod_idx] = p_mod; + } +} + +void SkeletonModificationStack3D::set_modification_count(int p_count) { + modifications.resize(p_count); + notify_property_list_changed(); +} + +int SkeletonModificationStack3D::get_modification_count() const { + return modifications.size(); +} + +void SkeletonModificationStack3D::set_skeleton(Skeleton3D *p_skeleton) { + skeleton = p_skeleton; +} + +Skeleton3D *SkeletonModificationStack3D::get_skeleton() const { + return skeleton; +} + +bool SkeletonModificationStack3D::get_is_setup() const { + return is_setup; +} + +void SkeletonModificationStack3D::set_enabled(bool p_enabled) { + enabled = p_enabled; + + if (!enabled && is_setup && skeleton != nullptr) { + skeleton->clear_bones_local_pose_override(); + } +} + +bool SkeletonModificationStack3D::get_enabled() const { + return enabled; +} + +void SkeletonModificationStack3D::set_strength(real_t p_strength) { + ERR_FAIL_COND_MSG(p_strength < 0, "Strength cannot be less than zero!"); + ERR_FAIL_COND_MSG(p_strength > 1, "Strength cannot be more than one!"); + strength = p_strength; +} + +real_t SkeletonModificationStack3D::get_strength() const { + return strength; +} + +void SkeletonModificationStack3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("setup"), &SkeletonModificationStack3D::setup); + ClassDB::bind_method(D_METHOD("execute", "delta", "execution_mode"), &SkeletonModificationStack3D::execute); + + ClassDB::bind_method(D_METHOD("enable_all_modifications", "enabled"), &SkeletonModificationStack3D::enable_all_modifications); + ClassDB::bind_method(D_METHOD("get_modification", "mod_idx"), &SkeletonModificationStack3D::get_modification); + ClassDB::bind_method(D_METHOD("add_modification", "modification"), &SkeletonModificationStack3D::add_modification); + ClassDB::bind_method(D_METHOD("delete_modification", "mod_idx"), &SkeletonModificationStack3D::delete_modification); + ClassDB::bind_method(D_METHOD("set_modification", "mod_idx", "modification"), &SkeletonModificationStack3D::set_modification); + + ClassDB::bind_method(D_METHOD("set_modification_count"), &SkeletonModificationStack3D::set_modification_count); + ClassDB::bind_method(D_METHOD("get_modification_count"), &SkeletonModificationStack3D::get_modification_count); + + ClassDB::bind_method(D_METHOD("get_is_setup"), &SkeletonModificationStack3D::get_is_setup); + + ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &SkeletonModificationStack3D::set_enabled); + ClassDB::bind_method(D_METHOD("get_enabled"), &SkeletonModificationStack3D::get_enabled); + + ClassDB::bind_method(D_METHOD("set_strength", "strength"), &SkeletonModificationStack3D::set_strength); + ClassDB::bind_method(D_METHOD("get_strength"), &SkeletonModificationStack3D::get_strength); + + ClassDB::bind_method(D_METHOD("get_skeleton"), &SkeletonModificationStack3D::get_skeleton); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "get_enabled"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "strength", PROPERTY_HINT_RANGE, "0, 1, 0.001"), "set_strength", "get_strength"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "modification_count", PROPERTY_HINT_RANGE, "0, 100, 1"), "set_modification_count", "get_modification_count"); +} + +SkeletonModificationStack3D::SkeletonModificationStack3D() { +} diff --git a/scene/resources/skeleton_modification_stack_3d.h b/scene/resources/skeleton_modification_stack_3d.h new file mode 100644 index 0000000000..cbc8d4e0b9 --- /dev/null +++ b/scene/resources/skeleton_modification_stack_3d.h @@ -0,0 +1,91 @@ +/*************************************************************************/ +/* skeleton_modification_stack_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 SKELETONMODIFICATIONSTACK3D_H +#define SKELETONMODIFICATIONSTACK3D_H + +#include "core/templates/local_vector.h" +#include "scene/3d/skeleton_3d.h" + +class Skeleton3D; +class SkeletonModification3D; + +class SkeletonModificationStack3D : public Resource { + GDCLASS(SkeletonModificationStack3D, Resource); + friend class Skeleton3D; + friend class SkeletonModification3D; + +protected: + static void _bind_methods(); + virtual void _get_property_list(List<PropertyInfo> *p_list) const; + virtual bool _set(const StringName &p_path, const Variant &p_value); + virtual bool _get(const StringName &p_path, Variant &r_ret) const; + +public: + Skeleton3D *skeleton = nullptr; + bool is_setup = false; + bool enabled = false; + real_t strength = 1.0; + + enum EXECUTION_MODE { + execution_mode_process, + execution_mode_physics_process, + }; + + LocalVector<Ref<SkeletonModification3D>> modifications = LocalVector<Ref<SkeletonModification3D>>(); + int modifications_count = 0; + + virtual void setup(); + virtual void execute(real_t p_delta, int p_execution_mode); + + void enable_all_modifications(bool p_enable); + Ref<SkeletonModification3D> get_modification(int p_mod_idx) const; + void add_modification(Ref<SkeletonModification3D> p_mod); + void delete_modification(int p_mod_idx); + void set_modification(int p_mod_idx, Ref<SkeletonModification3D> p_mod); + + void set_modification_count(int p_count); + int get_modification_count() const; + + void set_skeleton(Skeleton3D *p_skeleton); + Skeleton3D *get_skeleton() const; + + bool get_is_setup() const; + + void set_enabled(bool p_enabled); + bool get_enabled() const; + + void set_strength(real_t p_strength); + real_t get_strength() const; + + SkeletonModificationStack3D(); +}; + +#endif // SKELETONMODIFICATIONSTACK3D_H diff --git a/scene/scene_string_names.cpp b/scene/scene_string_names.cpp index b8173c9623..5d89d295c2 100644 --- a/scene/scene_string_names.cpp +++ b/scene/scene_string_names.cpp @@ -63,6 +63,7 @@ SceneStringNames::SceneStringNames() { animation_started = StaticCString::create("animation_started"); pose_updated = StaticCString::create("pose_updated"); + bone_pose_changed = StaticCString::create("bone_pose_changed"); mouse_entered = StaticCString::create("mouse_entered"); mouse_exited = StaticCString::create("mouse_exited"); diff --git a/scene/scene_string_names.h b/scene/scene_string_names.h index eddb0c33eb..01f427ecd1 100644 --- a/scene/scene_string_names.h +++ b/scene/scene_string_names.h @@ -98,6 +98,7 @@ public: StringName animation_started; StringName pose_updated; + StringName bone_pose_changed; StringName body_shape_entered; StringName body_entered; |