Path: blob/master/scene/resources/2d/skeleton/skeleton_modification_2d_twoboneik.cpp
21733 views
/**************************************************************************/1/* skeleton_modification_2d_twoboneik.cpp */2/**************************************************************************/3/* This file is part of: */4/* GODOT ENGINE */5/* https://godotengine.org */6/**************************************************************************/7/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */8/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */9/* */10/* Permission is hereby granted, free of charge, to any person obtaining */11/* a copy of this software and associated documentation files (the */12/* "Software"), to deal in the Software without restriction, including */13/* without limitation the rights to use, copy, modify, merge, publish, */14/* distribute, sublicense, and/or sell copies of the Software, and to */15/* permit persons to whom the Software is furnished to do so, subject to */16/* the following conditions: */17/* */18/* The above copyright notice and this permission notice shall be */19/* included in all copies or substantial portions of the Software. */20/* */21/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */22/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */23/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */24/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */25/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */26/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */27/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */28/**************************************************************************/2930#include "skeleton_modification_2d_twoboneik.h"31#include "scene/2d/skeleton_2d.h"3233#ifdef TOOLS_ENABLED34#include "editor/settings/editor_settings.h"35#endif // TOOLS_ENABLED3637bool SkeletonModification2DTwoBoneIK::_set(const StringName &p_path, const Variant &p_value) {38String path = p_path;3940if (path == "joint_one_bone_idx") {41set_joint_one_bone_idx(p_value);42} else if (path == "joint_one_bone2d_node") {43set_joint_one_bone2d_node(p_value);44} else if (path == "joint_two_bone_idx") {45set_joint_two_bone_idx(p_value);46} else if (path == "joint_two_bone2d_node") {47set_joint_two_bone2d_node(p_value);48}49#ifdef TOOLS_ENABLED50else if (path.begins_with("editor/draw_gizmo")) {51set_editor_draw_gizmo(p_value);52} else if (path.begins_with("editor/draw_min_max")) {53set_editor_draw_min_max(p_value);54}55#endif // TOOLS_ENABLED56else {57return false;58}5960return true;61}6263bool SkeletonModification2DTwoBoneIK::_get(const StringName &p_path, Variant &r_ret) const {64String path = p_path;6566if (path == "joint_one_bone_idx") {67r_ret = get_joint_one_bone_idx();68} else if (path == "joint_one_bone2d_node") {69r_ret = get_joint_one_bone2d_node();70} else if (path == "joint_two_bone_idx") {71r_ret = get_joint_two_bone_idx();72} else if (path == "joint_two_bone2d_node") {73r_ret = get_joint_two_bone2d_node();74}75#ifdef TOOLS_ENABLED76else if (path.begins_with("editor/draw_gizmo")) {77r_ret = get_editor_draw_gizmo();78} else if (path.begins_with("editor/draw_min_max")) {79r_ret = get_editor_draw_min_max();80}81#endif // TOOLS_ENABLED82else {83return false;84}8586return true;87}8889void SkeletonModification2DTwoBoneIK::_get_property_list(List<PropertyInfo> *p_list) const {90p_list->push_back(PropertyInfo(Variant::INT, "joint_one_bone_idx", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT));91p_list->push_back(PropertyInfo(Variant::NODE_PATH, "joint_one_bone2d_node", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Bone2D", PROPERTY_USAGE_DEFAULT));9293p_list->push_back(PropertyInfo(Variant::INT, "joint_two_bone_idx", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT));94p_list->push_back(PropertyInfo(Variant::NODE_PATH, "joint_two_bone2d_node", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Bone2D", PROPERTY_USAGE_DEFAULT));9596#ifdef TOOLS_ENABLED97if (Engine::get_singleton()->is_editor_hint()) {98p_list->push_back(PropertyInfo(Variant::BOOL, "editor/draw_gizmo", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT));99p_list->push_back(PropertyInfo(Variant::BOOL, "editor/draw_min_max", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT));100}101#endif // TOOLS_ENABLED102}103104void SkeletonModification2DTwoBoneIK::_execute(float p_delta) {105ERR_FAIL_COND_MSG(!stack || !is_setup || stack->skeleton == nullptr,106"Modification is not setup and therefore cannot execute!");107if (!enabled) {108return;109}110111if (target_node_cache.is_null()) {112WARN_PRINT_ONCE("Target cache is out of date. Attempting to update...");113update_target_cache();114return;115}116117if (joint_one_bone2d_node_cache.is_null() && !joint_one_bone2d_node.is_empty()) {118WARN_PRINT_ONCE("Joint one Bone2D node cache is out of date. Attempting to update...");119update_joint_one_bone2d_cache();120}121if (joint_two_bone2d_node_cache.is_null() && !joint_two_bone2d_node.is_empty()) {122WARN_PRINT_ONCE("Joint two Bone2D node cache is out of date. Attempting to update...");123update_joint_two_bone2d_cache();124}125126Node2D *target = ObjectDB::get_instance<Node2D>(target_node_cache);127if (!target || !target->is_inside_tree()) {128ERR_PRINT_ONCE("Target node is not in the scene tree. Cannot execute modification!");129return;130}131132Bone2D *joint_one_bone = stack->skeleton->get_bone(joint_one_bone_idx);133if (joint_one_bone == nullptr) {134ERR_PRINT_ONCE("Joint one bone_idx does not point to a valid bone! Cannot execute modification!");135return;136}137138Bone2D *joint_two_bone = stack->skeleton->get_bone(joint_two_bone_idx);139if (joint_two_bone == nullptr) {140ERR_PRINT_ONCE("Joint two bone_idx does not point to a valid bone! Cannot execute modification!");141return;142}143144// Adapted from the links below:145// http://theorangeduck.com/page/simple-two-joint146// https://www.alanzucconi.com/2018/05/02/ik-2d-2/147// With modifications by TwistedTwigleg148Vector2 target_difference = target->get_global_position() - joint_one_bone->get_global_position();149float joint_one_to_target = target_difference.length();150float angle_atan = target_difference.angle();151152float bone_one_length = joint_one_bone->get_length() * MIN(joint_one_bone->get_global_scale().abs().x, joint_one_bone->get_global_scale().abs().y);153float bone_two_length = joint_two_bone->get_length() * MIN(joint_two_bone->get_global_scale().abs().x, joint_two_bone->get_global_scale().abs().y);154bool override_angles_due_to_out_of_range = false;155bool same_scale_sign = true;156157if (joint_one_to_target < target_minimum_distance) {158joint_one_to_target = target_minimum_distance;159}160if (joint_one_to_target > target_maximum_distance && target_maximum_distance > 0.0) {161joint_one_to_target = target_maximum_distance;162}163164if (bone_one_length + bone_two_length < joint_one_to_target) {165override_angles_due_to_out_of_range = true;166}167168if (joint_one_bone->get_global_scale().sign().x != joint_one_bone->get_global_scale().sign().y) {169same_scale_sign = false;170}171172if (!override_angles_due_to_out_of_range) {173float angle_0 = Math::acos(((joint_one_to_target * joint_one_to_target) + (bone_one_length * bone_one_length) - (bone_two_length * bone_two_length)) / (2.0 * joint_one_to_target * bone_one_length));174float angle_1 = Math::acos(((bone_two_length * bone_two_length) + (bone_one_length * bone_one_length) - (joint_one_to_target * joint_one_to_target)) / (2.0 * bone_two_length * bone_one_length));175176if (flip_bend_direction) {177angle_0 = -angle_0;178angle_1 = -angle_1;179}180181if (std::isnan(angle_0) || std::isnan(angle_1)) {182// We cannot solve for this angle! Do nothing to avoid setting the rotation (and scale) to NaN.183} else {184if (same_scale_sign) {185joint_one_bone->set_global_rotation(angle_atan - angle_0 - joint_one_bone->get_bone_angle());186} else {187joint_one_bone->set_global_rotation(angle_atan + angle_0 + joint_one_bone->get_bone_angle());188}189190joint_two_bone->set_rotation(-Math::PI - angle_1 - joint_two_bone->get_bone_angle() + joint_one_bone->get_bone_angle());191}192193} else {194if (same_scale_sign) {195joint_one_bone->set_global_rotation(angle_atan - joint_one_bone->get_bone_angle());196joint_two_bone->set_global_rotation(angle_atan - joint_two_bone->get_bone_angle());197} else {198joint_one_bone->set_global_rotation(angle_atan + joint_one_bone->get_bone_angle());199joint_two_bone->set_global_rotation(angle_atan + joint_two_bone->get_bone_angle());200}201}202203stack->skeleton->set_bone_local_pose_override(joint_one_bone_idx, joint_one_bone->get_transform(), stack->strength, true);204stack->skeleton->set_bone_local_pose_override(joint_two_bone_idx, joint_two_bone->get_transform(), stack->strength, true);205}206207void SkeletonModification2DTwoBoneIK::_setup_modification(SkeletonModificationStack2D *p_stack) {208stack = p_stack;209210if (stack) {211is_setup = true;212update_target_cache();213update_joint_one_bone2d_cache();214update_joint_two_bone2d_cache();215}216}217218void SkeletonModification2DTwoBoneIK::_draw_editor_gizmo() {219if (!enabled || !is_setup || joint_one_bone_idx < 0) {220return;221}222223Bone2D *operation_bone_one = stack->skeleton->get_bone(joint_one_bone_idx);224if (!operation_bone_one) {225return;226}227stack->skeleton->draw_set_transform(228stack->skeleton->to_local(operation_bone_one->get_global_position()),229operation_bone_one->get_global_rotation() - stack->skeleton->get_global_rotation(),230operation_bone_one->get_global_scale());231232Color bone_ik_color = Color(1.0, 0.65, 0.0, 0.4);233#ifdef TOOLS_ENABLED234if (Engine::get_singleton()->is_editor_hint()) {235bone_ik_color = EDITOR_GET("editors/2d/bone_ik_color");236}237#endif // TOOLS_ENABLED238239if (flip_bend_direction) {240float angle = -(Math::PI * 0.5) + operation_bone_one->get_bone_angle();241stack->skeleton->draw_line(Vector2(0, 0), Vector2(Math::cos(angle), std::sin(angle)) * (operation_bone_one->get_length() * 0.5), bone_ik_color, 2.0);242} else {243float angle = (Math::PI * 0.5) + operation_bone_one->get_bone_angle();244stack->skeleton->draw_line(Vector2(0, 0), Vector2(Math::cos(angle), std::sin(angle)) * (operation_bone_one->get_length() * 0.5), bone_ik_color, 2.0);245}246247#ifdef TOOLS_ENABLED248if (Engine::get_singleton()->is_editor_hint()) {249if (editor_draw_min_max) {250if (target_maximum_distance != 0.0 || target_minimum_distance != 0.0) {251Vector2 target_direction = Vector2(0, 1);252if (target_node_cache.is_valid()) {253stack->skeleton->draw_set_transform(Vector2(0, 0), 0.0);254Node2D *target = ObjectDB::get_instance<Node2D>(target_node_cache);255target_direction = operation_bone_one->get_global_position().direction_to(target->get_global_position());256}257258stack->skeleton->draw_circle(target_direction * target_minimum_distance, 8, bone_ik_color);259stack->skeleton->draw_circle(target_direction * target_maximum_distance, 8, bone_ik_color);260stack->skeleton->draw_line(target_direction * target_minimum_distance, target_direction * target_maximum_distance, bone_ik_color, 2.0);261}262}263}264#endif // TOOLS_ENABLED265}266267void SkeletonModification2DTwoBoneIK::update_target_cache() {268if (!is_setup || !stack) {269if (is_setup) {270ERR_PRINT_ONCE("Cannot update target cache: modification is not properly setup!");271}272return;273}274275target_node_cache = ObjectID();276if (stack->skeleton) {277if (stack->skeleton->is_inside_tree()) {278if (stack->skeleton->has_node(target_node)) {279Node *node = stack->skeleton->get_node(target_node);280ERR_FAIL_COND_MSG(!node || stack->skeleton == node,281"Cannot update target cache: node is this modification's skeleton or cannot be found!");282ERR_FAIL_COND_MSG(!node->is_inside_tree(),283"Cannot update target cache: node is not in the scene tree!");284target_node_cache = node->get_instance_id();285}286}287}288}289290void SkeletonModification2DTwoBoneIK::update_joint_one_bone2d_cache() {291if (!is_setup || !stack) {292if (is_setup) {293ERR_PRINT_ONCE("Cannot update joint one Bone2D cache: modification is not properly setup!");294}295return;296}297298joint_one_bone2d_node_cache = ObjectID();299if (stack->skeleton) {300if (stack->skeleton->is_inside_tree()) {301if (stack->skeleton->has_node(joint_one_bone2d_node)) {302Node *node = stack->skeleton->get_node(joint_one_bone2d_node);303ERR_FAIL_COND_MSG(!node || stack->skeleton == node,304"Cannot update joint one Bone2D cache: node is this modification's skeleton or cannot be found!");305ERR_FAIL_COND_MSG(!node->is_inside_tree(),306"Cannot update joint one Bone2D cache: node is not in the scene tree!");307joint_one_bone2d_node_cache = node->get_instance_id();308309Bone2D *bone = Object::cast_to<Bone2D>(node);310if (bone) {311joint_one_bone_idx = bone->get_index_in_skeleton();312} else {313ERR_FAIL_MSG("Update joint one Bone2D cache: Nodepath to Bone2D is not a Bone2D node!");314}315}316}317}318}319320void SkeletonModification2DTwoBoneIK::update_joint_two_bone2d_cache() {321if (!is_setup || !stack) {322if (is_setup) {323ERR_PRINT_ONCE("Cannot update joint two Bone2D cache: modification is not properly setup!");324}325return;326}327328joint_two_bone2d_node_cache = ObjectID();329if (stack->skeleton) {330if (stack->skeleton->is_inside_tree()) {331if (stack->skeleton->has_node(joint_two_bone2d_node)) {332Node *node = stack->skeleton->get_node(joint_two_bone2d_node);333ERR_FAIL_COND_MSG(!node || stack->skeleton == node,334"Cannot update joint two Bone2D cache: node is this modification's skeleton or cannot be found!");335ERR_FAIL_COND_MSG(!node->is_inside_tree(),336"Cannot update joint two Bone2D cache: node is not in scene tree!");337joint_two_bone2d_node_cache = node->get_instance_id();338339Bone2D *bone = Object::cast_to<Bone2D>(node);340if (bone) {341joint_two_bone_idx = bone->get_index_in_skeleton();342} else {343ERR_FAIL_MSG("Update joint two Bone2D cache: Nodepath to Bone2D is not a Bone2D node!");344}345}346}347}348}349350void SkeletonModification2DTwoBoneIK::set_target_node(const NodePath &p_target_node) {351target_node = p_target_node;352update_target_cache();353}354355NodePath SkeletonModification2DTwoBoneIK::get_target_node() const {356return target_node;357}358359void SkeletonModification2DTwoBoneIK::set_joint_one_bone2d_node(const NodePath &p_target_node) {360joint_one_bone2d_node = p_target_node;361update_joint_one_bone2d_cache();362notify_property_list_changed();363}364365void SkeletonModification2DTwoBoneIK::set_target_minimum_distance(float p_distance) {366ERR_FAIL_COND_MSG(p_distance < 0, "Target minimum distance cannot be less than zero!");367target_minimum_distance = p_distance;368}369370float SkeletonModification2DTwoBoneIK::get_target_minimum_distance() const {371return target_minimum_distance;372}373374void SkeletonModification2DTwoBoneIK::set_target_maximum_distance(float p_distance) {375ERR_FAIL_COND_MSG(p_distance < 0, "Target maximum distance cannot be less than zero!");376target_maximum_distance = p_distance;377}378379float SkeletonModification2DTwoBoneIK::get_target_maximum_distance() const {380return target_maximum_distance;381}382383void SkeletonModification2DTwoBoneIK::set_flip_bend_direction(bool p_flip_direction) {384flip_bend_direction = p_flip_direction;385386#ifdef TOOLS_ENABLED387if (stack && is_setup) {388stack->set_editor_gizmos_dirty(true);389}390#endif // TOOLS_ENABLED391}392393bool SkeletonModification2DTwoBoneIK::get_flip_bend_direction() const {394return flip_bend_direction;395}396397NodePath SkeletonModification2DTwoBoneIK::get_joint_one_bone2d_node() const {398return joint_one_bone2d_node;399}400401void SkeletonModification2DTwoBoneIK::set_joint_two_bone2d_node(const NodePath &p_target_node) {402joint_two_bone2d_node = p_target_node;403update_joint_two_bone2d_cache();404notify_property_list_changed();405}406407NodePath SkeletonModification2DTwoBoneIK::get_joint_two_bone2d_node() const {408return joint_two_bone2d_node;409}410411void SkeletonModification2DTwoBoneIK::set_joint_one_bone_idx(int p_bone_idx) {412ERR_FAIL_COND_MSG(p_bone_idx < 0, "Bone index is out of range: The index is too low!");413414if (is_setup) {415if (stack->skeleton) {416ERR_FAIL_INDEX_MSG(p_bone_idx, stack->skeleton->get_bone_count(), "Passed-in Bone index is out of range!");417joint_one_bone_idx = p_bone_idx;418joint_one_bone2d_node_cache = stack->skeleton->get_bone(p_bone_idx)->get_instance_id();419joint_one_bone2d_node = stack->skeleton->get_path_to(stack->skeleton->get_bone(p_bone_idx));420} else {421WARN_PRINT("TwoBoneIK: Cannot verify the joint bone index for joint one...");422joint_one_bone_idx = p_bone_idx;423}424} else {425joint_one_bone_idx = p_bone_idx;426}427428notify_property_list_changed();429}430431int SkeletonModification2DTwoBoneIK::get_joint_one_bone_idx() const {432return joint_one_bone_idx;433}434435void SkeletonModification2DTwoBoneIK::set_joint_two_bone_idx(int p_bone_idx) {436ERR_FAIL_COND_MSG(p_bone_idx < 0, "Bone index is out of range: The index is too low!");437438if (is_setup) {439if (stack->skeleton) {440ERR_FAIL_INDEX_MSG(p_bone_idx, stack->skeleton->get_bone_count(), "Passed-in Bone index is out of range!");441joint_two_bone_idx = p_bone_idx;442joint_two_bone2d_node_cache = stack->skeleton->get_bone(p_bone_idx)->get_instance_id();443joint_two_bone2d_node = stack->skeleton->get_path_to(stack->skeleton->get_bone(p_bone_idx));444} else {445WARN_PRINT("TwoBoneIK: Cannot verify the joint bone index for joint two...");446joint_two_bone_idx = p_bone_idx;447}448} else {449joint_two_bone_idx = p_bone_idx;450}451452notify_property_list_changed();453}454455int SkeletonModification2DTwoBoneIK::get_joint_two_bone_idx() const {456return joint_two_bone_idx;457}458459#ifdef TOOLS_ENABLED460void SkeletonModification2DTwoBoneIK::set_editor_draw_min_max(bool p_draw) {461editor_draw_min_max = p_draw;462}463464bool SkeletonModification2DTwoBoneIK::get_editor_draw_min_max() const {465return editor_draw_min_max;466}467#endif // TOOLS_ENABLED468469void SkeletonModification2DTwoBoneIK::_bind_methods() {470ClassDB::bind_method(D_METHOD("set_target_node", "target_nodepath"), &SkeletonModification2DTwoBoneIK::set_target_node);471ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonModification2DTwoBoneIK::get_target_node);472473ClassDB::bind_method(D_METHOD("set_target_minimum_distance", "minimum_distance"), &SkeletonModification2DTwoBoneIK::set_target_minimum_distance);474ClassDB::bind_method(D_METHOD("get_target_minimum_distance"), &SkeletonModification2DTwoBoneIK::get_target_minimum_distance);475ClassDB::bind_method(D_METHOD("set_target_maximum_distance", "maximum_distance"), &SkeletonModification2DTwoBoneIK::set_target_maximum_distance);476ClassDB::bind_method(D_METHOD("get_target_maximum_distance"), &SkeletonModification2DTwoBoneIK::get_target_maximum_distance);477ClassDB::bind_method(D_METHOD("set_flip_bend_direction", "flip_direction"), &SkeletonModification2DTwoBoneIK::set_flip_bend_direction);478ClassDB::bind_method(D_METHOD("get_flip_bend_direction"), &SkeletonModification2DTwoBoneIK::get_flip_bend_direction);479480ClassDB::bind_method(D_METHOD("set_joint_one_bone2d_node", "bone2d_node"), &SkeletonModification2DTwoBoneIK::set_joint_one_bone2d_node);481ClassDB::bind_method(D_METHOD("get_joint_one_bone2d_node"), &SkeletonModification2DTwoBoneIK::get_joint_one_bone2d_node);482ClassDB::bind_method(D_METHOD("set_joint_one_bone_idx", "bone_idx"), &SkeletonModification2DTwoBoneIK::set_joint_one_bone_idx);483ClassDB::bind_method(D_METHOD("get_joint_one_bone_idx"), &SkeletonModification2DTwoBoneIK::get_joint_one_bone_idx);484485ClassDB::bind_method(D_METHOD("set_joint_two_bone2d_node", "bone2d_node"), &SkeletonModification2DTwoBoneIK::set_joint_two_bone2d_node);486ClassDB::bind_method(D_METHOD("get_joint_two_bone2d_node"), &SkeletonModification2DTwoBoneIK::get_joint_two_bone2d_node);487ClassDB::bind_method(D_METHOD("set_joint_two_bone_idx", "bone_idx"), &SkeletonModification2DTwoBoneIK::set_joint_two_bone_idx);488ClassDB::bind_method(D_METHOD("get_joint_two_bone_idx"), &SkeletonModification2DTwoBoneIK::get_joint_two_bone_idx);489490ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_nodepath", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node2D"), "set_target_node", "get_target_node");491ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "target_minimum_distance", PROPERTY_HINT_RANGE, "0,100000000,0.01,suffix:px"), "set_target_minimum_distance", "get_target_minimum_distance");492ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "target_maximum_distance", PROPERTY_HINT_NONE, "0,100000000,0.01,suffix:px"), "set_target_maximum_distance", "get_target_maximum_distance");493ADD_PROPERTY(PropertyInfo(Variant::BOOL, "flip_bend_direction", PROPERTY_HINT_NONE, ""), "set_flip_bend_direction", "get_flip_bend_direction");494ADD_GROUP("", "");495}496497SkeletonModification2DTwoBoneIK::SkeletonModification2DTwoBoneIK() {498stack = nullptr;499is_setup = false;500enabled = true;501editor_draw_gizmo = true;502}503504SkeletonModification2DTwoBoneIK::~SkeletonModification2DTwoBoneIK() {505}506507508