diff --git a/modules/constraint_ik/.github/ISSUE_TEMPLATE/bug_report.md b/modules/constraint_ik/.github/ISSUE_TEMPLATE/bug_report.md deleted file mode 100644 index 9b77ea713f98..000000000000 --- a/modules/constraint_ik/.github/ISSUE_TEMPLATE/bug_report.md +++ /dev/null @@ -1,40 +0,0 @@ ---- -name: Bug report -about: Create a report to help us improve -title: "" -labels: "" -assignees: "" ---- - -**Describe the bug** -A clear and concise description of what the bug is. - -**To Reproduce** -Steps to reproduce the behavior: - -1. Go to '...' -2. Click on '....' -3. Scroll down to '....' -4. See error - -**Expected behavior** -A clear and concise description of what you expected to happen. - -**Screenshots** -If applicable, add screenshots to help explain your problem. - -**Desktop (please complete the following information):** - -- OS: [e.g. iOS] -- Browser [e.g. chrome, safari] -- Version [e.g. 22] - -**Smartphone (please complete the following information):** - -- Device: [e.g. iPhone6] -- OS: [e.g. iOS8.1] -- Browser [e.g. stock browser, safari] -- Version [e.g. 22] - -**Additional context** -Add any other context about the problem here. diff --git a/modules/constraint_ik/.github/ISSUE_TEMPLATE/feature_request.md b/modules/constraint_ik/.github/ISSUE_TEMPLATE/feature_request.md deleted file mode 100644 index 2bc5d5f71186..000000000000 --- a/modules/constraint_ik/.github/ISSUE_TEMPLATE/feature_request.md +++ /dev/null @@ -1,19 +0,0 @@ ---- -name: Feature request -about: Suggest an idea for this project -title: "" -labels: "" -assignees: "" ---- - -**Is your feature request related to a problem? Please describe.** -A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] - -**Describe the solution you'd like** -A clear and concise description of what you want to happen. - -**Describe alternatives you've considered** -A clear and concise description of any alternative solutions or features you've considered. - -**Additional context** -Add any other context or screenshots about the feature request here. diff --git a/modules/constraint_ik/.gitignore b/modules/constraint_ik/.gitignore deleted file mode 100644 index 3946cc96e540..000000000000 --- a/modules/constraint_ik/.gitignore +++ /dev/null @@ -1,380 +0,0 @@ -# Godot .gitignore config -# -# Aims to encompass the most commonly found files that we don't want committed -# to Git, such as compilation output, IDE specific files, etc. -# -# It doesn't cover *all* thirdparty IDE extensions under the sun so if you have -# specific needs covered here, you can add them to: -# .git/info/exclude -# -# Or contribute them to this file if they're common enough that a good number of -# users would benefit from the shared rules. -# -# This file is organized by sections, with subsections ordered alphabetically. -# - Build configuration -# - Godot generated files -# - General build output -# - IDE and tool specific -# - Visual Studio specific -# - OS specific - -########################### -### Build configuration ### -########################### - -/custom.py -misc/hooks/pre-commit-custom-* - -############################# -### Godot generated files ### -############################# - -# Buildsystem -bin -*.gen.* -compile_commands.json -platform/windows/godot_res.res - -# Ninja build files -build.ninja -.ninja -run_ninja_env.bat - -# Generated by Godot binary -.import/ -/gdextension_interface.h -extension_api.json -logs/ - -# Generated by unit tests -tests/data/*.translation -tests/data/crypto/out* - -############################ -### General build output ### -############################ - -# C/C++ generated -*.a -*.ax -*.d -*.dll -*.lib -*.lo -*.o -*.os -*.ox -*.Plo -*.so -# Binutils tmp linker output of the form "stXXXXXX" where "X" is alphanumeric -st[A-Za-z0-9][A-Za-z0-9][A-Za-z0-9][A-Za-z0-9][A-Za-z0-9][A-Za-z0-9] - -# Python development -.venv -venv - -# Python generated -__pycache__/ -*.pyc - -# Documentation -doc/_build/ - -# Android -.gradle/ -local.properties -*.iml -.gradletasknamecache -project.properties -platform/android/java/*/.cxx/ -platform/android/java/*/build/ -platform/android/java/*/libs/ - -# iOS -*.dSYM - -# Web platform -*.bc -platform/web/node_modules/ - -# Misc -*.debug - -############################# -### IDE and tool specific ### -############################# - -# Automake -.deps/* -.dirstamp - -# ccls -.ccls-cache/ - -# clangd -.clangd/ -.cache/ - -# CLion -cmake-build-debug - -# Code::Blocks -*.cbp -*.layout -*.depend - -# CodeLite -*.project -*.workspace -.codelite/ - -# Cppcheck -*.cppcheck -cppcheck-cppcheck-build-dir/ - -# Eclipse CDT -.cproject -.settings/ -*.pydevproject -*.launch - -# Emacs -\#*\# -.\#* - -# GCOV code coverage -*.gcda -*.gcno - -# Geany -*.geany -.geanyprj - -# Gprof -gmon.out - -# Jetbrains IDEs -.idea/ -.fleet/ - -# Kate -*.kate-swp - -# Kdevelop -*.kdev4 - -# Mypy -.mypy_cache - -# Qt Creator -*.config -*.creator -*.creator.* -*.files -*.includes -*.cflags -*.cxxflags - -# SCons -.sconf_temp -.sconsign*.dblite -.scons_env.json -.scons_node_count - -# Sourcetrail -*.srctrl* - -# Tags -# https://github.com/github/gitignore/blob/master/Global/Tags.gitignore -# Ignore tags created by etags, ctags, gtags (GNU global) and cscope -TAGS -!TAGS/ -tags -*.tags -!tags/ -gtags.files -GTAGS -GRTAGS -GPATH -cscope.files -cscope.out -cscope.in.out -cscope.po.out - -# Vim -*.swo -*.swp - -# Visual Studio Code -.vscode/ -*.code-workspace -.history/ - -# Xcode -xcuserdata/ -*.xcscmblueprint -*.xccheckout -*.xcodeproj/* - -############################## -### Visual Studio specific ### -############################## - -# https://github.com/github/gitignore/blob/master/VisualStudio.gitignore -# Ignore Visual Studio temporary files, build results, and -# files generated by popular Visual Studio add-ons. - -# Actual VS project files we don't use -*.sln -*.vcxproj* - -# User-specific files -*.rsuser -*.suo -*.user -*.userosscache -*.sln.docstates - -# User-specific files (MonoDevelop/Xamarin Studio) -*.userprefs - -# Build results -[Dd]ebug/ -[Dd]ebugPublic/ -[Rr]elease/ -[Rr]eleases/ -x64/ -x86/ - -[Ww][Ii][Nn]32/ -[Aa][Rr][Mm]/ -[Aa][Rr][Mm]64/ -bld/ -[Bb]in/ -[Oo]bj/ -[Ll]og/ -[Ll]ogs/ - -# Do not ignore arch-specific folders anywhere under thirdparty libraries -!thirdparty/**/x64/ -!thirdparty/**/x86/ -!thirdparty/**/arm/ -!thirdparty/**/arm64/ - -# Visual Studio 2015/2017 cache/options directory -.vs/ - -# Visual Studio 2017 auto generated files -Generated\ Files/ - -# Files built by Visual Studio -*_i.c -*_p.c -*_h.h -*.ilk -*.meta -*.obj -*.iobj -*.pch -*.pdb -*.ipdb -*.pgc -*.pgd -*.rsp -*.sbr -*.tlb -*.tli -*.tlh -*.tmp -*.tmp_proj -*_wpftmp.csproj -*.log -*.tlog -*.vspscc -*.vssscc -.builds -*.pidb -*.svclog -*.scc - -# Visual C++ cache files -ipch/ -*.aps -*.ncb -*.opendb -*.opensdf -*.sdf -*.cachefile -*.VC.db -*.VC.VC.opendb - -# Visual Studio profiler -*.psess -*.vsp -*.vspx -*.sap - -# Visual Studio Trace Files -*.e2e - -# ReSharper is a .NET coding add-in -_ReSharper*/ -*.[Rr]e[Ss]harper -*.DotSettings.user - -# Visual Studio cache files -# files ending in .cache can be ignored -*.[Cc]ache - -# Others -ClientBin/ -enc_temp_folder/ -~$* -*.dbmdl -*.dbproj.schemaview -*.jfm -*.pfx -*.publishsettings -orleans.codegen.cs - -# Backup & report files from converting an old project file -# to a newer Visual Studio version. Backup files are not needed, -# because we have git ;-) -_UpgradeReport_Files/ -Backup*/ -UpgradeLog*.XML -UpgradeLog*.htm -ServiceFabricBackup/ -*.rptproj.bak - -# Hint file for IntelliSense -cpp.hint - -################### -### OS specific ### -################### - -# Linux -*~ -.directory - -# macOS -.DS_Store -__MACOSX - -# Windows -# https://github.com/github/gitignore/blob/main/Global/Windows.gitignore -[Tt]humbs.db -[Tt]humbs.db:encryptable -ehthumbs.db -ehthumbs_vista.db -*.stackdump -[Dd]esktop.ini -$RECYCLE.BIN/ -*.cab -*.msi -*.msix -*.msm -*.msp -*.lnk -*.generated.props diff --git a/modules/constraint_ik/LICENSE b/modules/constraint_ik/LICENSE deleted file mode 100644 index 24a6b933f27a..000000000000 --- a/modules/constraint_ik/LICENSE +++ /dev/null @@ -1,23 +0,0 @@ -MIT License - -Copyright (c) 2014-2022 Eron Gjoni -Copyright (c) 2019-2022 K. S. Ernest (iFire) Lee -Copyright (c) 2021 Rafael Martinez Gordillo. - -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. diff --git a/modules/constraint_ik/SCsub b/modules/constraint_ik/SCsub deleted file mode 100644 index 715fb0101706..000000000000 --- a/modules/constraint_ik/SCsub +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env python - -Import("env") - -env_many_bone_ik = env.Clone() -env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik"]) -env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik/src/math"]) -env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik/src"]) -env_many_bone_ik.add_source_files(env.modules_sources, "constraints/*.cpp") -env_many_bone_ik.add_source_files(env.modules_sources, "src/math/*.cpp") -env_many_bone_ik.add_source_files(env.modules_sources, "src/*.cpp") -env_many_bone_ik.add_source_files(env.modules_sources, "*.cpp") - -if env.editor_build: - env_many_bone_ik.add_source_files(env.modules_sources, "editor/*.cpp") diff --git a/modules/constraint_ik/config.py b/modules/constraint_ik/config.py deleted file mode 100644 index 488d7a0f9819..000000000000 --- a/modules/constraint_ik/config.py +++ /dev/null @@ -1,24 +0,0 @@ -def can_build(env, platform): - return not env["disable_3d"] - - -def configure(env): - pass - - -def get_doc_classes(): - return [ - "ConstraintIK3D", - "IKConstraintBone3D", - "IKConstraintEffector3D", - "IKBoneSegment3D", - "IKEffectorTemplate3D", - "IKKusudama3D", - "IKRay3D", - "IKConstraintNode3D", - "IKLimitCone3D", - ] - - -def get_doc_path(): - return "doc_classes" diff --git a/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CB22CAE13E99E3E24B.png b/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CB22CAE13E99E3E24B.png deleted file mode 100644 index 8db02055081d..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CB22CAE13E99E3E24B.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CB360C51B9B2145043.png b/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CB360C51B9B2145043.png deleted file mode 100644 index a565d772d250..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CB360C51B9B2145043.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CBF393B4C3206D97F3.png b/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CBF393B4C3206D97F3.png deleted file mode 100644 index 8be46c6e6ebc..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/1000000000000139000000CBF393B4C3206D97F3.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/100000000000016C000002AAA2059A0E43B2B468.png b/modules/constraint_ik/design_docs/Pictures/100000000000016C000002AAA2059A0E43B2B468.png deleted file mode 100644 index 9a8fa6af5bb3..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/100000000000016C000002AAA2059A0E43B2B468.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E1025A91C2576C186.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E1025A91C2576C186.png deleted file mode 100644 index 0daccbefc874..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E1025A91C2576C186.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E173E86C323891A74.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E173E86C323891A74.png deleted file mode 100644 index e45691aee337..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E173E86C323891A74.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E6996B4A5DF706332.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E6996B4A5DF706332.png deleted file mode 100644 index 40a36acf88a8..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E6996B4A5DF706332.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E832AFC8FF38BF53F.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E832AFC8FF38BF53F.png deleted file mode 100644 index fe5c87f9b02f..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E832AFC8FF38BF53F.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E845771BD15F39314.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E845771BD15F39314.png deleted file mode 100644 index 871a661f3aa8..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E845771BD15F39314.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E853FACED42A51700.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E853FACED42A51700.png deleted file mode 100644 index e53044de435e..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E853FACED42A51700.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E9D87E7B297A94C9F.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E9D87E7B297A94C9F.png deleted file mode 100644 index d18d3a42cc54..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018E9D87E7B297A94C9F.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EA6B7A87AEB9D9F40.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EA6B7A87AEB9D9F40.png deleted file mode 100644 index f77f5b60d8d0..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EA6B7A87AEB9D9F40.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EBC567E962B9B4C87.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EBC567E962B9B4C87.png deleted file mode 100644 index 2919df40c0ce..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EBC567E962B9B4C87.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018ECB3181C74F9EDC35.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018ECB3181C74F9EDC35.png deleted file mode 100644 index d30d4927b56c..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018ECB3181C74F9EDC35.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018ED76B18851DB9DC34.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018ED76B18851DB9DC34.png deleted file mode 100644 index 9373407d7bb5..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018ED76B18851DB9DC34.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EDE8A6B18141900B0.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EDE8A6B18141900B0.png deleted file mode 100644 index efb7b31856f7..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EDE8A6B18141900B0.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EDF01F2A95A1519D7.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EDF01F2A95A1519D7.png deleted file mode 100644 index c11b75c6c1be..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EDF01F2A95A1519D7.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EE63F4ECE1A82B606.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EE63F4ECE1A82B606.png deleted file mode 100644 index 196f99804799..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EE63F4ECE1A82B606.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EEE6FC1E017012EDA.png b/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EEE6FC1E017012EDA.png deleted file mode 100644 index 66ab6e24eb43..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002070000018EEE6FC1E017012EDA.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002C90000027876C8370C9358FB94.png b/modules/constraint_ik/design_docs/Pictures/10000000000002C90000027876C8370C9358FB94.png deleted file mode 100644 index 50e9f91d321a..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002C90000027876C8370C9358FB94.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002C900000278AEBB2DF17488D353.png b/modules/constraint_ik/design_docs/Pictures/10000000000002C900000278AEBB2DF17488D353.png deleted file mode 100644 index 4618a5411f08..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002C900000278AEBB2DF17488D353.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000002C900000278EEE925F5104E07BE.png b/modules/constraint_ik/design_docs/Pictures/10000000000002C900000278EEE925F5104E07BE.png deleted file mode 100644 index a2263757b5ba..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000002C900000278EEE925F5104E07BE.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000000000004000000040014FDC3377E0AD7E4.png b/modules/constraint_ik/design_docs/Pictures/10000000000004000000040014FDC3377E0AD7E4.png deleted file mode 100644 index 9acbcd9dd982..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000000000004000000040014FDC3377E0AD7E4.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/1000000000000400000004007DA1871E5E883934.png b/modules/constraint_ik/design_docs/Pictures/1000000000000400000004007DA1871E5E883934.png deleted file mode 100644 index ca6ceecb5be6..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/1000000000000400000004007DA1871E5E883934.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/10000201000002020000018EF75E743EDC6A705E.png b/modules/constraint_ik/design_docs/Pictures/10000201000002020000018EF75E743EDC6A705E.png deleted file mode 100644 index a949b58c73f1..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/10000201000002020000018EF75E743EDC6A705E.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/1000020100000286000001D10AA6EC3093AC56D5.png b/modules/constraint_ik/design_docs/Pictures/1000020100000286000001D10AA6EC3093AC56D5.png deleted file mode 100644 index 86258c569625..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/1000020100000286000001D10AA6EC3093AC56D5.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/Pictures/100002010000030D0000026B7F70BE2E85BEF4BE.png b/modules/constraint_ik/design_docs/Pictures/100002010000030D0000026B7F70BE2E85BEF4BE.png deleted file mode 100644 index f473a770fa7b..000000000000 Binary files a/modules/constraint_ik/design_docs/Pictures/100002010000030D0000026B7F70BE2E85BEF4BE.png and /dev/null differ diff --git a/modules/constraint_ik/design_docs/readme.md b/modules/constraint_ik/design_docs/readme.md deleted file mode 100644 index a3105edae229..000000000000 --- a/modules/constraint_ik/design_docs/readme.md +++ /dev/null @@ -1,555 +0,0 @@ -# EWBIK: A Highly General, Fast, Constrainable, and Stable Inverse Kinematics algorithm - -_Eron Gjoni_ - -This document introduces Entirely Wahba's-problem Based Inverse -Kinematics (EWBIK). EWBIK is fast and stable and remains so under -arbitrary joint orientation constraints, multiple end-effectors, -intermediary effectors, position and orientation targets of arbitrary -target weight priority, and any mix of the aforementioned. This document -additionally introduces Kusudama constraints, which provide a means for -fast, continuous, and expressive joint orientation constraints. A full -implementation of the EWB-IK algorithm and Kusudama constraint system is -available on the \*Everything WIll Be IK\* Java library github page^[^1]^, -a Processing demo implementation of the library is also available^[^2]^ -for quick visualization and use-case testing. - -## 0. Preamble and Authors: - -The Godot Engine many bone IK project is a port from a Java project by Eron Gjoni [Everything-Will-Be-IK](https://github.com/EGjoni/Everything-Will-Be-IK). - -The authors' GitHub usernames are indicated in parentheses. - -- Eron Gjoni (EGjoni) -- K. S. Ernest (iFire) Lee (fire) -- rafallus Rafael M. G. (rafallus) -- lyuma (lyuma) - -## 1. Introduction and Motivation: - -The two most popular approaches to the Inverse Kinematics problem for -interactive or real-time use cases are Cyclic Coordinate Descent^[^3]^ -(CCD) and Forward And Backward Reaching Inverse Kinematics (FABRIK). - -CCD offers highly stable and fast solutions that behave well under -joint constraints. However, CCD can solve only for single -target-effector pairs, and becomes highly unstable when negotiating -between multiple effectors aiming for potentially mutually exclusive -targets. - -FABRIK offers highly stable and fast solutions that naturally handle -multiple effectors with potentially mutually exclusive targets. -However, FABRIK is, in practice, extremely unstable when used with -joint constraints. - -Foregoing joint constraints to ensure stable FABRIK solutions results -in highly unnatural (often extremely painful) looking poses. While -foregoing consideration of more than one target-effector pair per bone -to ensure stable but well constrained CCD solutions results in -incomplete poses where, even if two separate bone chains with two -separate effectors could theoretically reach both of their targets -while obeying all constraints, only one effector actually does. - -From this, it should be apparent that the strengths and weaknesses of -these two approaches are approximate complements of one another, in -effect leaving developers to pick their poison. Additionally, both -approaches fail to meaningfully account for target orientations, which -can result in extremely unnatural pose solutions where the -end-effector must be rotated post-hoc to bear the brunt of orientation -alignment. - -## 2. The Importance of Orientation Targets: - -Consider a humanoid (perhaps yourself, if you happen to be -approximately humanoid in shape) sitting at a table, with the back of -its hand flat against the table such that its fingers are pointing -directly away from its torso. If the humanoid were instructed to -rotate its hand around its middle knuckle, such that the back of its -hand remained flat against the table, but its fingers now pointed -toward its torso; the orientation of all of the bones in the humanoid, -from its wrist up to its shoulder, and perhaps even part of its spine, -would have to change drastically to allow for this. - -If we treat the humanoid's pelvis as one effector and the chair as -that effector's target, and treat its knuckle bone as another -effector, and the spot on the table to which the knuckle bone must -remain affixed as the knuckle bone's target, we observe that even if -the *positions *of the targets do not change at all, there can be -massive differences in the poses an armature must adopt based solely -on the *orientations *of its targets. - -This should illustrate the importance of treating target orientations -as first class citizens throughout the entire IK procedure. If we -solve only for positions and leave target orientations as an -afterthought (as CCD and FABRIK implementations most often do) we are -left to decide between "cheating" by violating joint constraints so an -effector is still aligned with its target (often resulting in effector -joints that look like they're painfully hyperextending), or else -strictly obeying the joint constraints but failing to solve for an -otherwise reachable target. - -## 3. The Basic Idea: - -EWBIK can be thought of as a "blockwise" generalization of Inverse -Kinematics by CCD. The primary distinction is that, where CCD seeks to -iteratively minimize the angular discrepancy between a bone-chain's -end-effector and its corresponding target from the perspective of -every bone in the chain, EWBIK instead seeks to minimize an _average_ -discrepancy between _all_ effector-target pairs for *every *bone in -the chain. - -Broadly EWBIK starts from the outermost bones of an armature, and -proceeds rootward as follows: - -1. Create working copies of the origin and basis vectors and origins of - - all target and effector transforms relevant to a given bone, and - translate them along with the given bone transform's origin such - that the bone transform's origin is at (0,0,0). - -2. Find the rotation that minimizes the average of the discrepancy - - between each effector-target pair, and apply that rotation to the - bone. - -3. Rectify the bone's orientation to reside back within an allowable - - orientation as per any limits imposed by dampening parameters or - joint constraint on the bone if necessary, then proceed to the - next bone. - -4. Once the root bone has been reached, repeat the process starting - from the outermost bones until convergence or budget exhaustion. - -\*Figure 1.1 - 1.2: A simplified sketch of a step in the algorithm. -Effector basis vectors are marked in orange, target basis vectors are -marked in blue. Dashed magenta lines indicate the deltas between each -effector basis vector/origin and its corresponding target basis vector -/ origin. **\*⊕** _indicates the origin of the bone under consideration -at this step in the algorithm_. - -![](./Pictures/1000000000000400000004007DA1871E5E883934.png){width="3.0984in" -height="2.6563in"}![](./Pictures/10000000000004000000040014FDC3377E0AD7E4.png){width="3.1193in" -height="2.6457in"} - -
- -Figure 1.1 (left): \*Armature prior to rotation about **\*⊕** so as to -_minimize average discrepancies between effector points and -corresponding target points. _ - -Figure 1.2 (right): *Armature *after rotation about **⊕** so as to -minimize average *discrepancies *between all descendant effector points -and their corresponding target points. - -
- -
- -Naively, we might expect difficulties in the EWBIK procedure to arise -in step 2. However, step 2 amounts to a special case of _Wahba's -Problem_: that of finding the orthogonal transformation that best -aligns one set of vectors with a corresponding target set of vectors. -This type of problem arises often in bioinformatics, astronomy, -crystallography and computer vision, and can be solved extremely -quickly by any number of existing algorithms. Of these, the Kabsch -alignment algorithm and the Quaternion Characteristic Polynomial (QCP) -algorithms are perhaps the most popular. - -EWBIK has been verified to work fine with either algorithm, though QCP -is recommended for its stability, simplicity, framework agnosticism, -lack of edge-cases^[^4]^, and speed. - -Efficient implementations of both the QCP and Kabsch alignment -algorithms are widely available in a number of languages, and since -they (and related algorithms) are roughly interchangeable for our -purposes, their mathematical peculiarities will not be covered here, -and the rest of this document will refer to whatever algorithm you -choose for the purpose of minimizing the discrepancy between -point-pairs as _The Minimizer_. - -## 4. Role of The Minimizer: - -Chief among EWBIK's strengths is that no distinction is made by the -solver between position and orientation targets. Both orientation and -position are encoded simply as point pairs for the Minimizer to solve -over. - -This is achieved by representing each target as a set of up to 7 -points. One point representing the target origin, three points -representing the basis vectors emanating from that origin, and three -points representing the opposites of the basis vectors with respect to -the target origin. Effectors are represented in precisely the same -way. These 7 point-pairs are then fed to the minimizer, which attempts -to find the orthogonal transformation (usually just rotation) that -minimizes the average distance between all effector-target pairs. - -## 5. Multiple End and Intermediary-Effectors: - -\*\* \*\*Since the Minimizer blindly operates on point-pairs, generalizing -to solve for multiple effectors is trivial. We simply feed the Minimizer -all additional effector-target point-pairs for any other effectors we -wish to optimize a bone's orientation for. If the Minimizer optimizes -for the average euclidean distance between effector-target pairs, we can -even weigh some targets more strongly than others by just scaling the -effector-target pairs about the bone origin in proportion to the -precedence we want to place on that target-effector pair. This works -because rotation of any point closer to the origin results in a smaller -change in euclidean distance than does rotation of any point further -from the origin.^[^5]^ - -Additionally, we can weigh a target\'s orientation more or less -strongly than its position by scaling the basis vectors of the target -and/or effector about their respective origins. - -## 6. Preprocessing: - -When using EWBIK to solve for a single effector-target pair, no -preprocessing of the armature is required. However, if solving for -multiple effector-target pairs, the armature must be segmented prior to -solve time so as to ensure that ancestor bones are only solved for after -all of their descendant bones have been solved for, otherwise an -ancestor might end up minimizing for stale effector pairs as descendant -lineages have yet to finish solving. - -Such a problem scenario is depicted in _Figure 2.1_, the appropriate -segmentation of which is depicted in _Figure 2.2_. The rectangles -indicate logical segments. The numbers and letters indicate processing -order. With the only processing rule being that no bone of a greater -letter may be processed before any bone of a lesser letter, and no bone -of a greater number may be processed before any bone of a lesser number -in the same segment. - -![Figure 2.1](./Pictures/1000020100000286000001D10AA6EC3093AC56D5.png) -_Figure 2.1 (left): An example armature, with effectored bones indicated in orange._ - -![Figure 2.2](./Pictures/100002010000030D0000026B7F70BE2E85BEF4BE.png) -_Figure 2.2 (right): Segmented representation of the example armature in 2.1._ - -## 7. Solving: - -Once the segments have been created, we create working copies of the -basis vectors and origins representing all descendant target and -effector transforms on the segment, as well as opposing basis vectors -(copies of the original basis vectors flipped about their transform's -origin). Below, we will call any copies representing target basis -vectors and their opposites **_c_basisTargets_**, and any copies -representing effector basis vectors and their opposites -**_c_basisEffectors_**. We will call any copies representing origins -**_c_originTargets_** and **_c_originEffectors_**. - -From there, the simplest and fastest version of the EWBIK procedure -starts from the outermost segments and works inward to the root segment, -doing as follows for each bone in each segment: - -1. Reset the working copies of the targets and effector to correspond - - to their original values. Subtract the current bone's origin from - all _c_basisTarget_, *c_basisEffector, c_originTarget, *and* - c_originEffector * points. - - a. Scale any *c_basisTargets *about their corresponding *c_originTargets *such that their distance from their corresponding _c_originTarget_ is no less than 1, and also no less than the magnitude of their corresponding _c_originTarget, _ - - b. Scale any *c_basisEffectors *about their corresponding _c_originEffectors_ such that their distance from their corresponding _c_originEffectors_ is no less than 1, and no less than the magnitude of their corresponding _c_originTargets._ - -2. Use The Minimizer to compute the rotation that brings all - - _c\_\*Effector_ points as close as possible to their corresponding - _c\_\*Target_ points. - - a. Clamp this rotation by the desired dampening parameter. - - b. Apply the clamped rotation to the bone. - -3. Check if the bone has violated any of its orientation constraints as - - a result of this rotation. If it has, rotate the bone to reside - within a valid region of its orientation constraint. - -4. If the bone's parent is contained in the current segment, repeat - this process for the parent bone. Otherwise, traversal for this - segment is complete. - -Repeat the whole process until the armature has converged or the -computation budget has been exceeded. - -## 8. Constraints: - -\*\* \*\*In theory, EWBIK should work well with any type of commonly used -joint constraint (doing so requires no more than implementing step 3 in -the introductory section). Unfortunately, in practice, most commonly -used joint constraints come with their own set of tradeoffs. An ideal -orientation constraint system would provide the following - -1. **Continuity**: No sharp concave corners for a bone to get "stuck" in. - -2. **Versatility**: It should be possible to specify any conceivable orientation region. - -3. **Expressiveness**: The desired shape of the allowable orientation region should be fully specifiable with as few parameters as possible. - -4. **Speed**: as few operations as possible should be required to determine if a Bone is within the valid orientation region, or to determine the smallest rotation that brings it back within a valid orientation (note that this follows naturally from the previous criterion). - -5. **Extensibility**: The constraints should be amenable to specification of any number of additional properties that may vary continuously throughout or beyond the allowable orientations region (hard vs soft boundaries, high vs low friction regions, etc). - -The simplest conceivable systems for orientation constraints are Euler -angles, which offer speed, but not much else; and Reach cones, which -offer continuity, speed, and extensibility, but lack expressiveness or -versatility. - -More versatile constraint systems allow for per-vertex specification of -a polygonal bounding region on the surface of a sphere. Much like reach -cones, these operate on the principle that any point which resides -outside of the polygonal region should be transformed so as to reside on -the edge or vertex to which it is closest (see _figure 3.1 for a planar -representation_). -Unfortunately, the fewer edges the polygonal region is specified by, the greater the probability that it is closest to a vertex of the polygon than to an edge, which often results in the point getting "stuck" in corners (see _Figure 3.2 for a planar representation_). - -![Figure 3.1](./Pictures/10000000000002C900000278AEBB2DF17488D353.png) -![Figure 3.2](./Pictures/10000000000002C90000027876C8370C9358FB94.png) - -_Figure 3.1 (left): A sampling of points outside of the bounding region, with dotted lines indicating the area on the bounding region to which the constraint would transform them._ - -_Figure 3.2 (right): Colored areas indicate the edge to which any point within that area would be transformed so as to reside within the bounding polygon. Red regions indicate areas where all points would be transformed onto a single vertex._ - -These discontinuous corners can cause problems for an IK solver because they create local minima that are very difficult for solvers to find a way out of. Worse still, if the solver does get out, it tends to do so very suddenly, leading to jarring and unnatural "pops" between solutions. - -A common workaround is to smooth these corners out using splines or bezier curves, (see _Figure 4_ for a planar representation). However, while this solves the discontinuity problem, it does so at a significant performance penalty, because the only way to check whether or not a point on a sphere lies within the bounding spline is by segmenting the spline into very tiny linear segments, which then each have to be checked individually. - -![Figure 4](./Pictures/10000000000002C900000278EEE925F5104E07BE.png) -_Figure 4: A sampling of points outside of a continuous, spline-based bounding region, with dotted lines indicating the area on the bounding region to which the constraint would transform the point._ - -Aside from the performance penalty, the spline scheme is also somewhat strange conceptually in that it attempts to overcome the consequences of relying on a polygonal specification by adding an approximation of curvature by an increase in the number of line segments, and then mapping that approximation onto a sphere -- a domain in which curvature is already the rule, and linearity is inherently unnatural. - -If we start from scratch, and develop our bounding scheme with the upfront understanding that it will be mapped onto a sphere, instead of using points and lines as the fundamental units of our bounding region, we should prefer instead to think in terms of circles. Under such a scheme, a bounding region similar to that defined by the seven parameters (vertices) of Figure 5.1 might be represented as that defined by the six parameters (three circle centers, and three radii) of Figure 5.2. - -![Figure 5.1](./Pictures/10000000000002070000018EA6B7A87AEB9D9F40.png) -![Figure 5.2](./Pictures/10000000000002070000018EDE8A6B18141900B0.png) - -_Figure 5.1 (left): A polygonal bounding region, specified in terms of points and lines._ - -_Figure 5.2 (right): An approximation of the polygonal bounding region in Figure 5.1, specified as a connected sequence of circles of varying radii._ - -Note that because the bounding "lines" connecting any pair of circles are tangent to both circles, the entire boundary remains continuous. Of course, since we're mapping onto a sphere, these tangent "lines" are actually themselves circles of whatever radius is sufficient to contact both circle pairs (see Figure 6). Because there are an infinite number of circles which can contact two circles (both on the plane and on a sphere) we are also free to specify varying degrees of curvature to the regions bounding any two circles, as depicted in Figures 7.1 and 7.2. - -![Figure 6](./Pictures/100000000000016C000002AAA2059A0E43B2B468.png) -![Figure 7.1](./Pictures/10000201000002020000018EF75E743EDC6A705E.png) -![Figure 7.2](./Pictures/1000000000000139000000CB360C51B9B2145043.png) -![Figure 7.3](./Pictures/1000000000000139000000CBF393B4C3206D97F3.png) -![Figure 7.4](./Pictures/1000000000000139000000CB22CAE13E99E3E24B.png) - -_Figure 6: Spherical representation of bounding regions._ - -_Figure 7.1 (middle): Two circles (dark outline) and a sampling of the circles which lie tangent to both._ - -_Figure 7.2 (right): Result of choosing connecting circles of various radii._ - -These optional curvatures give us similar flexibility to that of the -spline approach, but need specify only one additional radius value per -pair of sequence-circles they connect (as there can only be at most two -tangent-circles satisfying a given radius). We'll look at the specifics -of representing our bounding region on a sphere in the next section, but -for now we'll limit ourselves to the plane so as to more easily -illustrate the form of the algorithm for checking whether we are within -the bounding region. - -We will presume our bounding region is made up of the three full circles -(which we will refer to as "sequence-circles") connected by a dotted -line depicted in _figure 8(a),_ and the six tangent-circles depicted in -light gray outlines in _figure_ _8(a2)_. - -1. We check to see if the point is within the two sequence-circles depicted in blue and green in _figure 8(b)_. - - a. If the point is within either sequence-circle, we terminate, as the point is within the allowable region. - - b. Otherwise, we proceed to step 2. - -2. We check to see if the point is within either of the two triangles depicted in amethyst in figure 8(c), which are formed by the centers of our pair of sequence-circle with the centers of the adjacent pair of tangent-circles. - - a. If the point is within either triangle, we proceed to step 3 - - b. Otherwise, we skip to step 4. - -3. We check to see if the point is within either of the adjacent tangent circles as depicted in _figure 8(d)_. - - a. If it is within one of the tangent-circles, then we transform it away from the center of the tangent-circle within which it resides such that its distance from the tangent-circle's center is equal to the radius of that tangent-circle (_figure 8(d2)_). Then terminate, as we have finished moving the point to the boundary of the allowable region. - - b. If it isn't within either circle, then proceed to step 4. - -4. Proceed to the next pair in the sequence (_figure 8(f)_), treating the blue sequence-circle from the previous steps as if it were the new green sequence-circle, and treating the next circle in the sequence as being the new blue sequence-circle. Repeat steps 1 through 4 (_figure 8(g)_) until the blue sequence-circle under consideration is the last one in the sequence, then proceed to step 6. - -5. If the point wasn't in any of the regions checked so far, then by process of elimination, it resides outside of any of the sequence circles, and the regions connecting the sequence circles, and anywhere which should be transformed to the regions connecting the sequence circles. So we just iterate through each sequence-circle individually, store the translation that would bring us (h) to its boundary, and apply whichever of the translations was smallest (i). - -| ![a](./Pictures/10000000000002070000018E853FACED42A51700.png) | ![a2](./Pictures/10000000000002070000018E1025A91C2576C186.png) | -| :-----------------------------------------------------------: | :------------------------------------------------------------: | -| (a) | (a2) | - -| ![b](./Pictures/10000000000002070000018E9D87E7B297A94C9F.png) | ![c](./Pictures/10000000000002070000018EE63F4ECE1A82B606.png) | -| :-----------------------------------------------------------: | :-----------------------------------------------------------: | -| (b) | (c) | - -| ![d](./Pictures/10000000000002070000018EDF01F2A95A1519D7.png) | ![d2](./Pictures/10000000000002070000018ED76B18851DB9DC34.png) | -| :-----------------------------------------------------------: | :------------------------------------------------------------: | -| (d) | (d2) | - -![f](./Pictures/10000000000002070000018EBC567E962B9B4C87.png) -(f) - -(g) -![g1](./Pictures/10000000000002070000018E832AFC8FF38BF53F.png) -![g2](./Pictures/10000000000002070000018ECB3181C74F9EDC35.png) -![g3](./Pictures/10000000000002070000018E173E86C323891A74.png) -![g4](./Pictures/10000000000002070000018EEE6FC1E017012EDA.png) - -| ![h](./Pictures/10000000000002070000018E6996B4A5DF706332.png) | ![i](./Pictures/10000000000002070000018E845771BD15F39314.png) | -| :-----------------------------------------------------------: | :-----------------------------------------------------------: | -| (h) | (i) | - -_Figure 8 (black regions indicate areas which have been eliminated from -further consideration by previous steps in the algorithm)_ - -## 9. Kusudamas: - -The spherical representation of such a bounding region uses cones -instead of circles. We replace the centers of the circles with vectors -pointing away from the constraint's origin, and replace their radii -with the half-angle of that cone's apex. - -This representation may be considered a generalization of the reach -cone concept. We call it a "Kusudama", as it resembles the spherical -japanese papercraft models made by sewing or glueing pyramidal units -through a common vertex. - -We are free to choose the apex angle of our tangent cones (and -therefore the curvature of the bounding region connecting our -sequence-cones) however we wish, so long as the following properties -are met. - -1. As the sum of the apex angles of our pair of sequence-cones - - approaches zero, the apex angles of the tangent cones must - approach π. - -2. As the sum of the apex angles of our pair of sequence-cones - approaches π, the apex angles of the tangent cones must approach 0. - -A straightforward function for automatically determining a reasonable -tangent cone radius in absence of any information aside from the radii -of the two sequence cones being connected is - -$$\frac{\pi - {({\theta{a + \theta}b})}}{2}$$ - -Where $$\theta a$$ and $$\theta b$$ are the apex angles of the two -sequence-cones being connected. - -Once we've determined an appropriate radius for a given tangent-cone, -all that's left is to find the direction in which it should point. To -do this, take the vectors representing the axis of the sequence-cones -being connected, and scale them to a magnitude of -$$\cos\left(\frac{\theta_{s} + \theta_{t}}{2}\right)$$ -Where $\theta_{s}$ is the apex angle of the sequence-cone to which the -vector corresponds, and $\theta_{t}$ is the apex angle of the -tangent cone we're determining the direction of. The two planes which -run through and are perpendicular to the tips of these scaled axis -vectors will intersect on a line running through the unit sphere. The -two points where this line intersects the unit sphere may be treated -as vectors representing the directions of our tangent-cone axes. - -Our full procedure for checking collisions is much the same as in the -planar case, with only minor modifications to account for the change in -topology. It goes as follows: - -1. We check to see if the angle between the bone's direction and the - - direction of the axis of each sequence-cone is less than the apex - angle of the sequence cones under consideration. - - a. If it is, we terminate, as the bone orientation is within the allowable region. - - b. Otherwise, we proceed to step 2. - -2. For each adjacent pair of sequence cones, we check to see if the - - bone direction is within either of the tetrahedrons formed by the - constraint origin, the line connecting the vectors representing - the two sequence-cone axes to each other, and the lines connecting - each sequence-cone axis to each tangent cone axis. - - a. If the bone direction is within either tetrahedron, we proceed to step 3 - - b. Otherwise, we skip to step 4. - -3. We check to see if the angle between the bone's direction and the direction of the axis of the tangent-cone coinciding with this tetrahedron is less than the apex half-angle of the tangent-cone under consideration. - - a. If it is, then we find the rotation which would transform the bone direction away from the tangent-cone in which it resides such that the angle between the bone direction and the tangent-cone's direction is equal to the apex half-angle of that tangent-cone. We do not terminate or apply this rotation. If the angle of this rotation is less than any currently stored rotation, we replace that rotation with this rotation, otherwise, we ignore this rotation. We then proceed to step 4. - - b. If it isn't, then proceed to step 4. - -4. We shift to the next pair of adjacent sequence cones and repeat steps 1 through 4 until one of the sequence cones under consideration is the last cone defined. Then proceed to step 5 - -5. We iterate again through each sequence-cone individually, and for each sequence-cone find the rotation which would transform the bone such that its angle from that sequence-cone axis is less than half the apex-angle of that sequence cone. We update the currently stored smallest rotation whenever we find a smaller rotation. (In effect, preferring to rotate the bone to the nearest constraint boundary) - -6. We apply the currently stored smallest rotation, and terminate. - -Note that as presented above, Kusudamas are only constraining the -direction (aka swing) of the bone. To constrain their axial orientation -(aka twist), a swing-twist decomposition may be used (as is common for -reach cone constraints). For best results, the constraint axis against -which twist is determined should be pointing as far away as possible -from the constraint\'s allowable region. - -**11. Robustness under enhancement:** - -In the canonical form presented above, Kusudumas empirically play quite -well with the EWBIK procedure. However, because the Kusudama scheme is -flexible enough to allow for "soft" or "springy" constraints, it is -possible to create a constraint landscape in which the solver undulates -around some optimum^[^6]^ . If it is absolutely critical that such -undulation be avoided, we can do so by incurring only a minor -performance penalty to check that the RMSD of our solution after -rotating and constraining a bone is less than or equal to our RMSD -before rotating and constraining the bone. If our RMSD is greater, we -simply discard our rotation + constraint transformation that step, and -allow the other bones in the chain to pick up the slack. - -
- -[^1]: [_https://github.com/EGjoni/Everything-Will-Be-IK_](https://github.com/EGjoni/Everything-Will-Be-IK) -[^2]: [_https://github.com/EGjoni/Everything-Will-Be-IK-Processing_](https://github.com/EGjoni/Everything-Will-Be-IK-Processing) -[^3]: - There is some terminological ambiguity as to what constitutes - "Coordinate Descent." Here, as is the case with the original CCD IK - algorithm, Coordinate Descent is used to refer to problems in which - each coordinate (e.g, a single variable at some cross section of a - multivariable function) can be precisely minimized. Cyclic - Coordinate Descent guarantees that so long as the function being - minimized is continuous and convex, successive minimization along - any permutation of its coordinates will converge on the global - minimum of the function (so long as evaluation of any coordinate is - preferred by no more than a constant factor). - - Procedures where multiple variables are minimized per iteration are - most often referred to as Blockwise Coordinate Descent. - -[^4]: - With single precision floating point, QCP may under some - degenerate conditions require renormalization of the resultant - quaternion. This renormalization does not affect solution quality, - but may be worth considering for anyone looking to hyper-optimize - performance, as it does require an additional square root operation. - In theory, double precision floating point should occasionally - require similar rectification, though for somewhat mysterious - reasons it never seems to in practice. - -[^5]: - This can occur incidentally, where one set of target-effector - point-pairs happens to be closer to the origin of a bone being - solved for than another set of target-effector point-pairs. For this - reason, the full EWBIK procedure scales the basis vectors of any - target and effector transforms by the distance between the target - transform's origin and the origin of the transform of the bone being - solved for. - -[^6]: - In theory, even the simple canonical form of kusudamas should be - subject to this. Though, in practice it's almost never an issue - until soft constraints enter the mix. diff --git a/modules/constraint_ik/doc_classes/ConstraintIK3D.xml b/modules/constraint_ik/doc_classes/ConstraintIK3D.xml deleted file mode 100644 index 0446de231a62..000000000000 --- a/modules/constraint_ik/doc_classes/ConstraintIK3D.xml +++ /dev/null @@ -1,189 +0,0 @@ - - - - A general inverse kinematics system with constraints. - - - The ConstraintIK3D class provides a comprehensive system for inverse kinematics (IK) with support for various constraints. It allows for complex IK setups involving multiple bones, each with their own constraints and parameters. - - - - - - - - - Returns the index of the constraint with the given name. If no such constraint exists, returns -1. - - - - - - Returns the total number of bones in the IK system. - - - - - - Returns the total number of constraints in the IK system. - - - - - - - Returns the name of the constraint at the specified index. - - - - - - - - - - - - - - - - - - - - Returns the center of the limit cone for the kusudama at the specified index. - - - - - - - Returns the count of limit cones for the kusudama at the specified index. - - - - - - - - Returns the radius of the limit cone for the kusudama at the specified index. - - - - - - - - - - - - - - - - - - Registers the skeleton to the IK system. This should be called after all bones and constraints have been added to the system. - - - - - - Resets all constraints in the IK system to their default state. - - - - - - - Sets the total number of constraints. - - - - - - - - - - - - - - - - - - - - Marks the IK system as dirty, indicating that it needs to be updated. This should be called whenever a significant change is made to the system. - - - - - - - - - - - - - - - - Sets the center of the limit cone for the kusudama at the specified index. - - - - - - - - Sets the count of limit cones for the kusudama at the specified index. - - - - - - - - - Sets the radius of the limit cone for the kusudama at the specified index. - - - - - - - - - - - - - - - - - - - - A boolean value indicating whether the IK system is in constraint mode or not. - - - The default maximum number of radians a bone is allowed to rotate per solver iteration. The lower this value, the more natural the pose results. However, this will increase the number of iterations_per_frame the solver requires to converge. - - - The number of iterations performed by the solver per frame. - - - The number of stabilization passes performed by the solver. This can help to improve the stability of the IK solution. - - - The index of the bone currently selected in the user interface. - - - diff --git a/modules/constraint_ik/doc_classes/IKBone3D.xml b/modules/constraint_ik/doc_classes/IKBone3D.xml deleted file mode 100644 index b9eb5a3db688..000000000000 --- a/modules/constraint_ik/doc_classes/IKBone3D.xml +++ /dev/null @@ -1,50 +0,0 @@ - - - - A class representing a 3D bone in an Inverse Kinematics system. - - - The IKConstraintBone3D class represents a 3D bone in an Inverse Kinematics (IK) system. It provides methods to get and set constraints, pins, and check if the bone is pinned. - - - - - - - - Returns the IKKusudama3D object representing the constraint of the bone. - - - - - - Returns the IKConstraintNode3D object representing the orientation transform of the constraint. - - - - - - Returns the IKConstraintNode3D object representing the twist transform of the constraint. - - - - - - Returns the IKConstraintEffector3D object representing the pin of the bone. - - - - - - Returns true if the bone is pinned, false otherwise. - - - - - - - Sets the IKConstraintEffector3D object representing the pin of the bone. - - - - diff --git a/modules/constraint_ik/doc_classes/IKBoneSegment3D.xml b/modules/constraint_ik/doc_classes/IKBoneSegment3D.xml deleted file mode 100644 index 945bcc51daa1..000000000000 --- a/modules/constraint_ik/doc_classes/IKBoneSegment3D.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - A class representing a 3D bone segment in an Inverse Kinematics system. - - - The IKBoneSegment3D class represents a 3D bone segment in an Inverse Kinematics (IK) system. It provides methods to get the IK bone associated with the segment and check if the segment is pinned. - - - - - - - - - Returns the IKConstraintBone3D object associated with the given bone index. - - - - - - Returns true if the bone segment is pinned, false otherwise. - - - - diff --git a/modules/constraint_ik/doc_classes/IKEffector3D.xml b/modules/constraint_ik/doc_classes/IKEffector3D.xml deleted file mode 100644 index 3e70aca6fae7..000000000000 --- a/modules/constraint_ik/doc_classes/IKEffector3D.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - A class representing a 3D effector in an Inverse Kinematics system. - - - The IKConstraintEffector3D class represents an effector in a 3D Inverse Kinematics (IK) system. An effector is the end point of a chain of bones, and its position is used to calculate the rotations of all preceding bones in the IK chain. This class provides methods for getting and setting the target node of the effector. - - - - - - Pins can be ultimate targets or intermediary targets. - By default, each pin is treated as an ultimate target, meaning any bones which are ancestors to that pin's effector are not aware of any pins which are the target of bones descending from that effector. - Changing this value makes ancestor bones aware and determines how much less they care with each level down. - Presuming all descendants of this pin have a falloff of 1, then: A pin falloff of 0 on this pin means only this pin is reported to ancestors. A pin falloff of 1 on this pin means ancestors care about all descendant pins equally (after accounting for their pin weight), regardless of how many levels down they are. A pin falloff of 0.5 means each descendant pin is used about half as much as its ancestor. The pin's falloff of a descendant is taken into account for each level with each level. - Meaning, if this pin has a falloff of 1 and its descendent has a falloff of 0.5, then it will be reported with total weight. Then, its descendant will be calculated with total weight; the descendant of that pin will be calculated with half weight. Finally, the descendant of that one's descendant will be with calculated quarter weight. - - - diff --git a/modules/constraint_ik/doc_classes/IKEffectorTemplate3D.xml b/modules/constraint_ik/doc_classes/IKEffectorTemplate3D.xml deleted file mode 100644 index 640dfd723bdb..000000000000 --- a/modules/constraint_ik/doc_classes/IKEffectorTemplate3D.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - A template class for creating 3D effectors in an Inverse Kinematics system. - - - The IKEffectorTemplate3D class provides a template for creating effectors in a 3D Inverse Kinematics (IK) system. It includes properties for direction priorities, passthrough factor, target node, and weight. These properties can be set to customize the behavior of the effector. - - - - - - - - - - - - - - - - - - - Specifies the priority of movement in each direction (X, Y, Z). Higher values indicate higher priority. - - - - - The weight of the effector. This determines how much the effector's position influences the IK calculation. Higher values result in greater influence. - - - diff --git a/modules/constraint_ik/doc_classes/IKNode3D.xml b/modules/constraint_ik/doc_classes/IKNode3D.xml deleted file mode 100644 index f46c01017b1f..000000000000 --- a/modules/constraint_ik/doc_classes/IKNode3D.xml +++ /dev/null @@ -1,87 +0,0 @@ - - - - A 3D node used for inverse kinematics calculations. - - - The IKConstraintNode3D class provides a node that can be used in an inverse kinematics chain. It includes methods for getting and setting the global and local transforms of the node, as well as disabling scaling and converting between local and global coordinates. - - - - - - - - Returns the global transform of this node. - - - - - - Returns the parent of this node. - - - - - - Returns the local transform of this node. - - - - - - Returns whether scaling is disabled for this node. - - - - - - - - Rotates the local transform of this node with a global basis. If propagate is true, the rotation is also applied to the children of this node. - - - - - - - Disables or enables scaling for this node. - - - - - - - Sets the global transform of this node. - - - - - - - Sets the parent of this node. - - - - - - - Sets the local transform of this node. - - - - - - - Converts a point from local space to global space. - - - - - - - Converts a point from global space to local space. - - - - diff --git a/modules/constraint_ik/register_types.cpp b/modules/constraint_ik/register_types.cpp deleted file mode 100644 index 1af389cdd6a5..000000000000 --- a/modules/constraint_ik/register_types.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************/ -/* register_types.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "register_types.h" - -#include "src/ik_bone_3d.h" -#include "src/ik_effector_3d.h" -#include "src/ik_effector_template_3d.h" -#include "src/ik_kusudama_3d.h" -#include "src/many_bone_ik_3d.h" - -#ifdef TOOLS_ENABLED -#include "editor/many_bone_ik_3d_gizmo_plugin.h" -#endif - -void initialize_constraint_ik_module(ModuleInitializationLevel p_level) { - if (p_level == MODULE_INITIALIZATION_LEVEL_SCENE) { - } -#ifdef TOOLS_ENABLED - if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) { - EditorPlugins::add_by_type(); - } -#endif - if (p_level == MODULE_INITIALIZATION_LEVEL_SERVERS) { - GDREGISTER_CLASS(IKConstraintEffectorTemplate3D); - GDREGISTER_CLASS(ConstraintIK3D); - GDREGISTER_CLASS(IKConstraintBone3D); - GDREGISTER_CLASS(IKConstraintNode3D); - GDREGISTER_CLASS(IKConstraintEffector3D); - GDREGISTER_CLASS(IKConstraintBoneSegment3D); - GDREGISTER_CLASS(IKKusudama3D); - GDREGISTER_CLASS(IKRay3D); - GDREGISTER_CLASS(IKLimitCone3D); - } -} - -void uninitialize_constraint_ik_module(ModuleInitializationLevel p_level) { - if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) { - return; - } -} diff --git a/modules/constraint_ik/register_types.h b/modules/constraint_ik/register_types.h deleted file mode 100644 index 4cc1f39d7f12..000000000000 --- a/modules/constraint_ik/register_types.h +++ /dev/null @@ -1,39 +0,0 @@ -/**************************************************************************/ -/* register_types.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef CONSTRAINT_IK_REGISTER_TYPES_H -#define CONSTRAINT_IK_REGISTER_TYPES_H - -#include "modules/register_module_types.h" - -void initialize_constraint_ik_module(ModuleInitializationLevel p_level); -void uninitialize_constraint_ik_module(ModuleInitializationLevel p_level); - -#endif // CONSTRAINT_IK_REGISTER_TYPES_H diff --git a/modules/constraint_ik/src/ik_bone_3d.cpp b/modules/constraint_ik/src/ik_bone_3d.cpp deleted file mode 100644 index ab790dbd62e1..000000000000 --- a/modules/constraint_ik/src/ik_bone_3d.cpp +++ /dev/null @@ -1,363 +0,0 @@ -/**************************************************************************/ -/* ik_bone_3d.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "ik_bone_3d.h" -#include "ik_kusudama_3d.h" -#include "many_bone_ik_3d.h" -#include "math/ik_node_3d.h" -#include - -void IKConstraintBone3D::set_bone_id(BoneId p_bone_id, Skeleton3D *p_skeleton) { - ERR_FAIL_NULL(p_skeleton); - bone_id = p_bone_id; -} - -BoneId IKConstraintBone3D::get_bone_id() const { - return bone_id; -} - -void IKConstraintBone3D::set_parent(const Ref &p_parent) { - ERR_FAIL_COND(p_parent.is_null()); - parent = p_parent; - if (parent.is_valid()) { - parent->children.push_back(this); - godot_skeleton_aligned_transform->set_parent(parent->godot_skeleton_aligned_transform); - constraint_orientation_transform->set_parent(parent->godot_skeleton_aligned_transform); - constraint_twist_transform->set_parent(parent->godot_skeleton_aligned_transform); - } -} - -void IKConstraintBone3D::update_default_bone_direction_transform(Skeleton3D *p_skeleton) { - Vector3 child_centroid; - int child_count = 0; - - for (Ref &ik_bone : children) { - child_centroid += ik_bone->get_ik_transform()->get_global_transform().origin; - child_count++; - } - - if (child_count > 0) { - child_centroid /= child_count; - } else { - const PackedInt32Array &bone_children = p_skeleton->get_bone_children(bone_id); - for (BoneId child_bone_idx : bone_children) { - child_centroid += p_skeleton->get_bone_global_pose(child_bone_idx).origin; - } - child_centroid /= bone_children.size(); - } - - const Vector3 &godot_bone_origin = godot_skeleton_aligned_transform->get_global_transform().origin; - child_centroid -= godot_bone_origin; - - if (Math::is_zero_approx(child_centroid.length_squared())) { - if (parent.is_valid()) { - child_centroid = parent->get_bone_direction_transform()->get_global_transform().basis.get_column(Vector3::AXIS_Y); - } else { - child_centroid = get_bone_direction_transform()->get_global_transform().basis.get_column(Vector3::AXIS_Y); - } - } - - if (!Math::is_zero_approx(child_centroid.length_squared()) && (children.size() || p_skeleton->get_bone_children(bone_id).size())) { - child_centroid.normalize(); - Vector3 bone_direction = bone_direction_transform->get_global_transform().basis.get_column(Vector3::AXIS_Y); - bone_direction.normalize(); - bone_direction_transform->rotate_local_with_global(Quaternion(child_centroid, bone_direction)); - } -} - -void IKConstraintBone3D::update_default_constraint_transform() { - Ref parent_bone = get_parent(); - if (parent_bone.is_valid()) { - Transform3D parent_bone_aligned_transform = get_parent_bone_aligned_transform(); - constraint_orientation_transform->set_global_transform(parent_bone_aligned_transform); - } - - Transform3D set_constraint_twist_transform = get_set_constraint_twist_transform(); - constraint_twist_transform->set_global_transform(set_constraint_twist_transform); - - if (constraint.is_null()) { - return; - } - - TypedArray cones = constraint->get_open_cones(); - Vector3 direction; - if (cones.size() == 0) { - direction = bone_direction_transform->get_global_transform().basis.get_column(Vector3::AXIS_Y); - } else { - float total_radius_sum = calculate_total_radius_sum(cones); - direction = calculate_weighted_direction(cones, total_radius_sum); - direction -= constraint_orientation_transform->get_global_transform().origin; - } - - Vector3 twist_axis = set_constraint_twist_transform.basis.get_column(Vector3::AXIS_Y); - Quaternion align_dir = Quaternion(twist_axis, direction); - constraint_twist_transform->rotate_local_with_global(align_dir); -} - -Ref IKConstraintBone3D::get_parent() const { - return parent; -} - -void IKConstraintBone3D::set_pin(const Ref &p_pin) { - ERR_FAIL_COND(p_pin.is_null()); - pin = p_pin; -} - -Ref IKConstraintBone3D::get_pin() const { - return pin; -} - -void IKConstraintBone3D::set_pose(const Transform3D &p_transform) { - godot_skeleton_aligned_transform->set_transform(p_transform); -} - -Transform3D IKConstraintBone3D::get_pose() const { - return godot_skeleton_aligned_transform->get_transform(); -} - -void IKConstraintBone3D::set_global_pose(const Transform3D &p_transform) { - godot_skeleton_aligned_transform->set_global_transform(p_transform); - Transform3D transform = constraint_orientation_transform->get_transform(); - transform.origin = godot_skeleton_aligned_transform->get_transform().origin; - constraint_orientation_transform->set_transform(transform); - constraint_orientation_transform->_propagate_transform_changed(); -} - -Transform3D IKConstraintBone3D::get_global_pose() const { - return godot_skeleton_aligned_transform->get_global_transform(); -} - -Transform3D IKConstraintBone3D::get_bone_direction_global_pose() const { - return bone_direction_transform->get_global_transform(); -} - -void IKConstraintBone3D::set_initial_pose(Skeleton3D *p_skeleton) { - ERR_FAIL_NULL(p_skeleton); - if (bone_id == -1) { - return; - } - Transform3D bone_origin_to_parent_origin = p_skeleton->get_bone_pose(bone_id); - set_pose(bone_origin_to_parent_origin); -} - -void IKConstraintBone3D::set_skeleton_bone_pose(Skeleton3D *p_skeleton) { - ERR_FAIL_NULL(p_skeleton); - Transform3D bone_to_parent = get_pose(); - p_skeleton->set_bone_pose_position(bone_id, bone_to_parent.origin); - if (!bone_to_parent.basis.is_finite()) { - bone_to_parent.basis = Basis(); - } - p_skeleton->set_bone_pose_rotation(bone_id, bone_to_parent.basis.get_rotation_quaternion()); - p_skeleton->set_bone_pose_scale(bone_id, bone_to_parent.basis.get_scale()); -} - -void IKConstraintBone3D::create_pin() { - pin = Ref(memnew(IKConstraintEffector3D(this))); -} - -bool IKConstraintBone3D::is_pinned() const { - return pin.is_valid(); -} - -void IKConstraintBone3D::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_pin"), &IKConstraintBone3D::get_pin); - ClassDB::bind_method(D_METHOD("set_pin", "pin"), &IKConstraintBone3D::set_pin); - ClassDB::bind_method(D_METHOD("is_pinned"), &IKConstraintBone3D::is_pinned); - ClassDB::bind_method(D_METHOD("get_constraint"), &IKConstraintBone3D::get_constraint); - ClassDB::bind_method(D_METHOD("get_constraint_orientation_transform"), &IKConstraintBone3D::get_constraint_orientation_transform); - ClassDB::bind_method(D_METHOD("get_constraint_twist_transform"), &IKConstraintBone3D::get_constraint_twist_transform); -} - -IKConstraintBone3D::IKConstraintBone3D(StringName p_bone, Skeleton3D *p_skeleton, const Ref &p_parent, Vector> &p_pins, float p_default_dampening, - ConstraintIK3D *p_many_bone_ik) { - ERR_FAIL_NULL(p_skeleton); - - default_dampening = p_default_dampening; - cos_half_dampen = cos(default_dampening / real_t(2.0)); - set_name(p_bone); - bone_id = p_skeleton->find_bone(p_bone); - if (p_parent.is_valid()) { - set_parent(p_parent); - } - for (Ref elem : p_pins) { - if (elem.is_null()) { - continue; - } - if (elem->get_name() == p_bone) { - create_pin(); - Ref effector = get_pin(); - effector->set_motion_propagation_factor(elem->get_motion_propagation_factor()); - effector->set_weight(elem->get_weight()); - effector->set_direction_priorities(elem->get_direction_priorities()); - break; - } - } - bone_direction_transform->set_parent(godot_skeleton_aligned_transform); - - float predamp = 1.0 - get_stiffness(); - dampening = get_parent().is_null() ? Math_PI : predamp * p_default_dampening; - float iterations = p_many_bone_ik->get_iterations_per_frame(); - if (get_constraint().is_null()) { - Ref new_constraint; - new_constraint.instantiate(); - add_constraint(new_constraint); - } - float returnfulness = get_constraint()->get_resistance(); - float falloff = 0.2f; - half_returnfulness_dampened.resize(iterations); - cos_half_returnfulness_dampened.resize(iterations); - float iterations_pow = Math::pow(iterations, falloff * iterations * returnfulness); - for (float i = 0; i < iterations; i++) { - float iteration_scalar = ((iterations_pow)-Math::pow(i, falloff * iterations * returnfulness)) / (iterations_pow); - float iteration_return_clamp = iteration_scalar * returnfulness * dampening; - float cos_iteration_return_clamp = Math::cos(iteration_return_clamp / 2.0); - half_returnfulness_dampened.write[i] = iteration_return_clamp; - cos_half_returnfulness_dampened.write[i] = cos_iteration_return_clamp; - } -} - -float IKConstraintBone3D::get_cos_half_dampen() const { - return cos_half_dampen; -} - -void IKConstraintBone3D::set_cos_half_dampen(float p_cos_half_dampen) { - cos_half_dampen = p_cos_half_dampen; -} - -Ref IKConstraintBone3D::get_constraint() const { - return constraint; -} - -void IKConstraintBone3D::add_constraint(Ref p_constraint) { - constraint = p_constraint; -} - -Ref IKConstraintBone3D::get_ik_transform() { - return godot_skeleton_aligned_transform; -} - -Ref IKConstraintBone3D::get_constraint_orientation_transform() { - return constraint_orientation_transform; -} - -Ref IKConstraintBone3D::get_constraint_twist_transform() { - return constraint_twist_transform; -} - -void IKConstraintBone3D::set_constraint_orientation_transform(Ref p_transform) { - constraint_orientation_transform = p_transform; -} - -void IKConstraintBone3D::set_bone_direction_transform(Ref p_bone_direction) { - bone_direction_transform = p_bone_direction; -} - -Ref IKConstraintBone3D::get_bone_direction_transform() { - return bone_direction_transform; -} - -bool IKConstraintBone3D::is_orientationally_constrained() { - if (get_constraint().is_null()) { - return false; - } - return get_constraint()->is_orientationally_constrained(); -} - -bool IKConstraintBone3D::is_axially_constrained() { - if (get_constraint().is_null()) { - return false; - } - return get_constraint()->is_axially_constrained(); -} - -Vector &IKConstraintBone3D::get_cos_half_returnfullness_dampened() { - return cos_half_returnfulness_dampened; -} - -void IKConstraintBone3D::set_cos_half_returnfullness_dampened(const Vector &p_value) { - cos_half_returnfulness_dampened = p_value; -} - -Vector &IKConstraintBone3D::get_half_returnfullness_dampened() { - return half_returnfulness_dampened; -} - -void IKConstraintBone3D::set_half_returnfullness_dampened(const Vector &p_value) { - half_returnfulness_dampened = p_value; -} - -void IKConstraintBone3D::set_stiffness(double p_stiffness) { - stiffness = p_stiffness; -} - -double IKConstraintBone3D::get_stiffness() const { - return stiffness; -} - -Transform3D IKConstraintBone3D::get_parent_bone_aligned_transform() { - Ref parent_bone = get_parent(); - if (parent_bone.is_null()) { - return Transform3D(); - } - Transform3D parent_bone_aligned_transform = parent_bone->get_ik_transform()->get_global_transform(); - parent_bone_aligned_transform.origin = get_bone_direction_transform()->get_global_transform().origin; - return parent_bone_aligned_transform; -} - -Transform3D IKConstraintBone3D::get_set_constraint_twist_transform() const { - return constraint_orientation_transform->get_global_transform(); -} - -float IKConstraintBone3D::calculate_total_radius_sum(const TypedArray &p_cones) const { - float total_radius_sum = 0.0f; - for (int32_t i = 0; i < p_cones.size(); ++i) { - const Ref &cone = p_cones[i]; - if (cone.is_null()) { - break; - } - total_radius_sum += cone->get_radius(); - } - return total_radius_sum; -} - -Vector3 IKConstraintBone3D::calculate_weighted_direction(const TypedArray &p_cones, float p_total_radius_sum) const { - Vector3 direction = Vector3(); - for (int32_t i = 0; i < p_cones.size(); ++i) { - const Ref &cone = p_cones[i]; - if (cone.is_null()) { - break; - } - float weight = cone->get_radius() / p_total_radius_sum; - direction += cone->get_control_point() * weight; - } - direction.normalize(); - direction = constraint_orientation_transform->get_global_transform().basis.xform(direction); - return direction; -} diff --git a/modules/constraint_ik/src/ik_bone_3d.h b/modules/constraint_ik/src/ik_bone_3d.h deleted file mode 100644 index 058a9cc7b329..000000000000 --- a/modules/constraint_ik/src/ik_bone_3d.h +++ /dev/null @@ -1,122 +0,0 @@ -/**************************************************************************/ -/* ik_bone_3d.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef IK_CONSTRAINT_BONE_3D_H -#define IK_CONSTRAINT_BONE_3D_H - -#include "ik_effector_template_3d.h" -#include "ik_kusudama_3d.h" -#include "ik_open_cone_3d.h" -#include "math/ik_node_3d.h" - -#include "core/io/resource.h" -#include "core/object/ref_counted.h" -#include "scene/3d/skeleton_3d.h" - -class IKConstraintEffector3D; -class ConstraintIK3D; -class IKConstraintBone3D; - -class IKConstraintBone3D : public Resource { - GDCLASS(IKConstraintBone3D, Resource); - - BoneId bone_id = -1; - Ref parent; - Vector> children; - Ref pin; - - float default_dampening = Math_PI; - float dampening = get_parent().is_null() ? Math_PI : default_dampening; - float cos_half_dampen = Math::cos(dampening / 2.0f); - double cos_half_return_damp = 0.0f; - double return_damp = 0.0f; - Vector cos_half_returnfulness_dampened; - Vector half_returnfulness_dampened; - double stiffness = 0.0; - Ref constraint; - // In the space of the local parent bone transform. - // The origin is the origin of the bone direction transform - // Can be independent and should be calculated - // to keep -y to be the opposite of its bone forward orientation - // To avoid singularity that is ambiguous. - Ref constraint_orientation_transform = Ref(memnew(IKConstraintNode3D())); - Ref constraint_twist_transform = Ref(memnew(IKConstraintNode3D())); - Ref godot_skeleton_aligned_transform = Ref(memnew(IKConstraintNode3D())); // The bone's actual transform. - Ref bone_direction_transform = Ref(memnew(IKConstraintNode3D())); // Physical direction of the bone. Calculate Y is the bone up. - -protected: - static void _bind_methods(); - -public: - Vector &get_cos_half_returnfullness_dampened(); - void set_cos_half_returnfullness_dampened(const Vector &p_value); - Vector &get_half_returnfullness_dampened(); - void set_half_returnfullness_dampened(const Vector &p_value); - void set_stiffness(double p_stiffness); - double get_stiffness() const; - bool is_axially_constrained(); - bool is_orientationally_constrained(); - Transform3D get_bone_direction_global_pose() const; - Ref get_bone_direction_transform(); - void set_bone_direction_transform(Ref p_bone_direction); - void update_default_bone_direction_transform(Skeleton3D *p_skeleton); - void set_constraint_orientation_transform(Ref p_transform); - Ref get_constraint_orientation_transform(); - Ref get_constraint_twist_transform(); - void update_default_constraint_transform(); - void add_constraint(Ref p_constraint); - Ref get_constraint() const; - void set_bone_id(BoneId p_bone_id, Skeleton3D *p_skeleton = nullptr); - BoneId get_bone_id() const; - void set_parent(const Ref &p_parent); - Ref get_parent() const; - void set_pin(const Ref &p_pin); - Ref get_pin() const; - void set_global_pose(const Transform3D &p_transform); - Transform3D get_global_pose() const; - void set_pose(const Transform3D &p_transform); - Transform3D get_pose() const; - void set_initial_pose(Skeleton3D *p_skeleton); - void set_skeleton_bone_pose(Skeleton3D *p_skeleton); - void create_pin(); - bool is_pinned() const; - Ref get_ik_transform(); - IKConstraintBone3D() {} - IKConstraintBone3D(StringName p_bone, Skeleton3D *p_skeleton, const Ref &p_parent, Vector> &p_pins, float p_default_dampening = Math_PI, ConstraintIK3D *p_many_bone_ik = nullptr); - ~IKConstraintBone3D() {} - float get_cos_half_dampen() const; - void set_cos_half_dampen(float p_cos_half_dampen); - Transform3D get_parent_bone_aligned_transform(); - Transform3D get_set_constraint_twist_transform() const; - float calculate_total_radius_sum(const TypedArray &p_cones) const; - Vector3 calculate_weighted_direction(const TypedArray &p_cones, float p_total_radius_sum) const; -}; - -#endif // IK_CONSTRAINT_BONE_3D_H diff --git a/modules/constraint_ik/src/ik_bone_segment_3d.cpp b/modules/constraint_ik/src/ik_bone_segment_3d.cpp deleted file mode 100644 index d249e3417c41..000000000000 --- a/modules/constraint_ik/src/ik_bone_segment_3d.cpp +++ /dev/null @@ -1,427 +0,0 @@ -/**************************************************************************/ -/* ik_bone_segment_3d.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "ik_bone_segment_3d.h" - -#include "core/string/string_builder.h" -#include "ik_effector_3d.h" -#include "ik_kusudama_3d.h" -#include "many_bone_ik_3d.h" -#include "scene/3d/skeleton_3d.h" - -Ref IKConstraintBoneSegment3D::get_root() const { - return root; -} - -Ref IKConstraintBoneSegment3D::get_tip() const { - return tip; -} - -bool IKConstraintBoneSegment3D::is_pinned() const { - ERR_FAIL_COND_V(tip.is_null(), false); - return tip->is_pinned(); -} - -Vector> IKConstraintBoneSegment3D::get_child_segments() const { - return child_segments; -} - -void IKConstraintBoneSegment3D::create_bone_list(Vector> &p_list, bool p_recursive) const { - if (p_recursive) { - for (int32_t child_i = 0; child_i < child_segments.size(); child_i++) { - child_segments[child_i]->create_bone_list(p_list, p_recursive); - } - } - Ref current_bone = tip; - Vector> list; - while (current_bone.is_valid()) { - list.push_back(current_bone); - if (current_bone == root) { - break; - } - current_bone = current_bone->get_parent(); - } - p_list.append_array(list); -} - -void IKConstraintBoneSegment3D::update_pinned_list(Vector> &r_weights) { - for (int32_t chain_i = 0; chain_i < child_segments.size(); chain_i++) { - Ref chain = child_segments[chain_i]; - chain->update_pinned_list(r_weights); - } - if (is_pinned()) { - effector_list.push_back(tip->get_pin()); - } - double motion_propagation_factor = is_pinned() ? tip->get_pin()->get_motion_propagation_factor() : 1.0; - if (motion_propagation_factor > 0.0) { - for (Ref child : child_segments) { - effector_list.append_array(child->effector_list); - } - } -} - -void IKConstraintBoneSegment3D::_update_optimal_rotation(Ref p_for_bone, double p_damp, bool p_translate, bool p_constraint_mode, int32_t current_iteration, int32_t total_iterations) { - ERR_FAIL_COND(p_for_bone.is_null()); - _update_target_headings(p_for_bone, &heading_weights, &target_headings); - _update_tip_headings(p_for_bone, &tip_headings); - _set_optimal_rotation(p_for_bone, &tip_headings, &target_headings, &heading_weights, p_damp, p_translate, p_constraint_mode); -} - -Quaternion IKConstraintBoneSegment3D::clamp_to_cos_half_angle(Quaternion p_quat, double p_cos_half_angle) { - if (p_quat.w < 0.0) { - p_quat = p_quat * -1; - } - double previous_coefficient = (1.0 - (p_quat.w * p_quat.w)); - if (p_cos_half_angle <= p_quat.w || previous_coefficient == 0.0) { - return p_quat; - } else { - double composite_coefficient = Math::sqrt((1.0 - (p_cos_half_angle * p_cos_half_angle)) / previous_coefficient); - p_quat.w = p_cos_half_angle; - p_quat.x *= composite_coefficient; - p_quat.y *= composite_coefficient; - p_quat.z *= composite_coefficient; - } - return p_quat; -} - -float IKConstraintBoneSegment3D::_get_manual_msd(const PackedVector3Array &r_htip, const PackedVector3Array &r_htarget, const Vector &p_weights) { - float manual_RMSD = 0.0f; - float w_sum = 0.0f; - for (int i = 0; i < r_htarget.size(); i++) { - float x_d = r_htarget[i].x - r_htip[i].x; - float y_d = r_htarget[i].y - r_htip[i].y; - float z_d = r_htarget[i].z - r_htip[i].z; - float mag_sq = p_weights[i] * (x_d * x_d + y_d * y_d + z_d * z_d); - manual_RMSD += mag_sq; - w_sum += p_weights[i]; - } - manual_RMSD /= w_sum * w_sum; - return manual_RMSD; -} - -void IKConstraintBoneSegment3D::_set_optimal_rotation(Ref p_for_bone, PackedVector3Array *r_htip, PackedVector3Array *r_htarget, Vector *r_weights, float p_dampening, bool p_translate, bool p_constraint_mode, double current_iteration, double total_iterations) { - ERR_FAIL_COND(p_for_bone.is_null()); - ERR_FAIL_NULL(r_htip); - ERR_FAIL_NULL(r_htarget); - ERR_FAIL_NULL(r_weights); - - _update_target_headings(p_for_bone, &heading_weights, &target_headings); - Transform3D prev_transform = p_for_bone->get_pose(); - bool got_closer = true; - double bone_damp = p_for_bone->get_cos_half_dampen(); - int i = 0; - do { - _update_tip_headings(p_for_bone, &tip_headings); - if (!p_constraint_mode) { - Array superpose_result = QuaternionCharacteristicPolynomial::weighted_superpose(*r_htip, *r_htarget, *r_weights, p_translate, evec_prec); - Quaternion rotation = superpose_result[0]; - Vector3 translation = superpose_result[1]; - double dampening = (p_dampening != -1.0) ? p_dampening : bone_damp; - rotation = clamp_to_cos_half_angle(rotation, cos(dampening / 2.0)); - if (current_iteration == 0) { - current_iteration = 0.0001; - } - rotation = rotation.slerp(p_for_bone->get_global_pose().basis, static_cast(total_iterations) / current_iteration); - p_for_bone->get_ik_transform()->rotate_local_with_global(rotation); - Transform3D result = Transform3D(p_for_bone->get_global_pose().basis, p_for_bone->get_global_pose().origin + translation); - p_for_bone->set_global_pose(result); - } - bool is_parent_valid = p_for_bone->get_parent().is_valid(); - if (is_parent_valid && p_for_bone->is_orientationally_constrained()) { - p_for_bone->get_constraint()->snap_to_orientation_limit(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_orientation_transform(), bone_damp, p_for_bone->get_cos_half_dampen()); - } - if (is_parent_valid && p_for_bone->is_axially_constrained()) { - p_for_bone->get_constraint()->set_snap_to_twist_limit(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_twist_transform(), bone_damp, p_for_bone->get_cos_half_dampen()); - } - if (default_stabilizing_pass_count > 0) { - _update_tip_headings(p_for_bone, &tip_headings_uniform); - double current_msd = _get_manual_msd(tip_headings_uniform, target_headings, heading_weights); - if (current_msd <= previous_deviation * 1.0001) { - previous_deviation = current_msd; - got_closer = true; - break; - } else { - got_closer = false; - p_for_bone->set_pose(prev_transform); - } - } - i++; - } while (i < default_stabilizing_pass_count && !got_closer); - - if (root == p_for_bone) { - previous_deviation = INFINITY; - } -} - -void IKConstraintBoneSegment3D::_update_target_headings(Ref p_for_bone, Vector *r_weights, PackedVector3Array *r_target_headings) { - ERR_FAIL_COND(p_for_bone.is_null()); - ERR_FAIL_NULL(r_weights); - ERR_FAIL_NULL(r_target_headings); - int32_t last_index = 0; - for (int32_t effector_i = 0; effector_i < effector_list.size(); effector_i++) { - Ref effector = effector_list[effector_i]; - if (effector.is_null()) { - continue; - } - last_index = effector->update_effector_target_headings(r_target_headings, last_index, p_for_bone, &heading_weights); - } -} - -void IKConstraintBoneSegment3D::_update_tip_headings(Ref p_for_bone, PackedVector3Array *r_heading_tip) { - ERR_FAIL_NULL(r_heading_tip); - ERR_FAIL_COND(p_for_bone.is_null()); - int32_t last_index = 0; - for (int32_t effector_i = 0; effector_i < effector_list.size(); effector_i++) { - Ref effector = effector_list[effector_i]; - if (effector.is_null()) { - continue; - } - last_index = effector->update_effector_tip_headings(r_heading_tip, last_index, p_for_bone); - } -} - -void IKConstraintBoneSegment3D::segment_solver(const Vector &p_damp, float p_default_damp, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iteration) { - for (Ref child : child_segments) { - if (child.is_null()) { - continue; - } - child->segment_solver(p_damp, p_default_damp, p_constraint_mode, p_current_iteration, p_total_iteration); - } - bool is_translate = parent_segment.is_null(); - if (is_translate) { - Vector damp = p_damp; - damp.fill(Math_PI); - _qcp_solver(damp, Math_PI, is_translate, p_constraint_mode, p_current_iteration, p_total_iteration); - return; - } - _qcp_solver(p_damp, p_default_damp, is_translate, p_constraint_mode, p_current_iteration, p_total_iteration); -} - -void IKConstraintBoneSegment3D::_qcp_solver(const Vector &p_damp, float p_default_damp, bool p_translate, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iterations) { - for (Ref current_bone : bones) { - float damp = p_default_damp; - bool is_valid_access = !(unlikely((p_damp.size()) < 0 || (current_bone->get_bone_id()) >= (p_damp.size()))); - if (is_valid_access) { - damp = p_damp[current_bone->get_bone_id()]; - } - bool is_non_default_damp = p_default_damp < damp; - if (is_non_default_damp) { - damp = p_default_damp; - } - _update_optimal_rotation(current_bone, damp, p_translate, p_constraint_mode, p_current_iteration, p_total_iterations); - } -} - -void IKConstraintBoneSegment3D::_bind_methods() { - ClassDB::bind_method(D_METHOD("is_pinned"), &IKConstraintBoneSegment3D::is_pinned); - ClassDB::bind_method(D_METHOD("get_ik_bone", "bone"), &IKConstraintBoneSegment3D::get_ik_bone); -} - -IKConstraintBoneSegment3D::IKConstraintBoneSegment3D(Skeleton3D *p_skeleton, StringName p_root_bone_name, Vector> &p_pins, ConstraintIK3D *p_many_bone_ik, const Ref &p_parent, - BoneId p_root, BoneId p_tip, int32_t p_stabilizing_pass_count) { - root = p_root; - tip = p_tip; - skeleton = p_skeleton; - root = Ref(memnew(IKConstraintBone3D(p_root_bone_name, p_skeleton, p_parent, p_pins, Math_PI, p_many_bone_ik))); - if (p_parent.is_valid()) { - root_segment = p_parent->root_segment; - } else { - root_segment = Ref(this); - } - root_segment->bone_map[root->get_bone_id()] = root; - if (p_parent.is_valid()) { - parent_segment = p_parent; - root->set_parent(p_parent->get_tip()); - } - default_stabilizing_pass_count = p_stabilizing_pass_count; -} - -void IKConstraintBoneSegment3D::_enable_pinned_descendants() { - pinned_descendants = true; -} - -bool IKConstraintBoneSegment3D::_has_pinned_descendants() { - return pinned_descendants; -} - -Ref IKConstraintBoneSegment3D::get_ik_bone(BoneId p_bone) const { - if (!bone_map.has(p_bone)) { - return Ref(); - } - return bone_map[p_bone]; -} - -void IKConstraintBoneSegment3D::create_headings_arrays() { - Vector> penalty_array; - Vector> new_pinned_bones; - recursive_create_penalty_array(this, penalty_array, new_pinned_bones, 1.0); - pinned_bones.resize(new_pinned_bones.size()); - int32_t total_headings = 0; - for (const Vector ¤t_penalty_array : penalty_array) { - total_headings += current_penalty_array.size(); - } - for (int32_t bone_i = 0; bone_i < new_pinned_bones.size(); bone_i++) { - pinned_bones.write[bone_i] = new_pinned_bones[bone_i]; - } - target_headings.resize(total_headings); - tip_headings.resize(total_headings); - tip_headings_uniform.resize(total_headings); - heading_weights.resize(total_headings); - int currentHeading = 0; - for (const Vector ¤t_penalty_array : penalty_array) { - for (double ad : current_penalty_array) { - heading_weights.write[currentHeading] = ad; - target_headings.write[currentHeading] = Vector3(); - tip_headings.write[currentHeading] = Vector3(); - tip_headings_uniform.write[currentHeading] = Vector3(); - currentHeading++; - } - } -} - -void IKConstraintBoneSegment3D::recursive_create_penalty_array(Ref p_bone_segment, Vector> &r_penalty_array, Vector> &r_pinned_bones, double p_falloff) { - if (p_falloff <= 0.0) { - return; - } - - double current_falloff = 1.0; - - if (p_bone_segment->is_pinned()) { - Ref current_tip = p_bone_segment->get_tip(); - Ref pin = current_tip->get_pin(); - double weight = pin->get_weight(); - Vector inner_weight_array; - inner_weight_array.push_back(weight * p_falloff); - - double max_pin_weight = MAX(MAX(pin->get_direction_priorities().x, pin->get_direction_priorities().y), pin->get_direction_priorities().z); - max_pin_weight = max_pin_weight == 0.0 ? 1.0 : max_pin_weight; - - for (int i = 0; i < 3; ++i) { - double priority = pin->get_direction_priorities()[i]; - if (priority > 0.0) { - double sub_target_weight = weight * (priority / max_pin_weight) * p_falloff; - inner_weight_array.push_back(sub_target_weight); - inner_weight_array.push_back(sub_target_weight); - } - } - - r_penalty_array.push_back(inner_weight_array); - r_pinned_bones.push_back(current_tip); - current_falloff = pin->get_motion_propagation_factor(); - } - - for (Ref s : p_bone_segment->get_child_segments()) { - recursive_create_penalty_array(s, r_penalty_array, r_pinned_bones, p_falloff * current_falloff); - } -} - -void IKConstraintBoneSegment3D::recursive_create_headings_arrays_for(Ref p_bone_segment) { - p_bone_segment->create_headings_arrays(); - for (Ref segments : p_bone_segment->get_child_segments()) { - recursive_create_headings_arrays_for(segments); - } -} - -void IKConstraintBoneSegment3D::generate_default_segments(Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ConstraintIK3D *p_many_bone_ik) { - Ref current_tip = root; - Vector children; - - while (!_is_parent_of_tip(current_tip, p_tip_bone)) { - children = skeleton->get_bone_children(current_tip->get_bone_id()); - - if (children.is_empty() || _has_multiple_children_or_pinned(children, current_tip)) { - _process_children(children, current_tip, p_pins, p_root_bone, p_tip_bone, p_many_bone_ik); - break; - } else { - Vector::Iterator bone_id_iterator = children.begin(); - current_tip = _create_next_bone(*bone_id_iterator, current_tip, p_pins, p_many_bone_ik); - } - } - - _finalize_segment(current_tip); -} - -bool IKConstraintBoneSegment3D::_is_parent_of_tip(Ref p_current_tip, BoneId p_tip_bone) { - return skeleton->get_bone_parent(p_current_tip->get_bone_id()) >= p_tip_bone && p_tip_bone != -1; -} - -bool IKConstraintBoneSegment3D::_has_multiple_children_or_pinned(Vector &r_children, Ref p_current_tip) { - return r_children.size() > 1 || p_current_tip->is_pinned(); -} - -void IKConstraintBoneSegment3D::_process_children(Vector &r_children, Ref p_current_tip, Vector> &r_pins, BoneId p_root_bone, BoneId p_tip_bone, ConstraintIK3D *p_many_bone_ik) { - tip = p_current_tip; - Ref parent(this); - - for (int32_t child_i = 0; child_i < r_children.size(); child_i++) { - BoneId child_bone = r_children[child_i]; - String child_name = skeleton->get_bone_name(child_bone); - Ref child_segment = _create_child_segment(child_name, r_pins, p_root_bone, p_tip_bone, p_many_bone_ik, parent); - - child_segment->generate_default_segments(r_pins, p_root_bone, p_tip_bone, p_many_bone_ik); - - if (child_segment->_has_pinned_descendants()) { - _enable_pinned_descendants(); - child_segments.push_back(child_segment); - } - } -} - -Ref IKConstraintBoneSegment3D::_create_child_segment(String &p_child_name, Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ConstraintIK3D *p_many_bone_ik, Ref &p_parent) { - return Ref(memnew(IKConstraintBoneSegment3D(skeleton, p_child_name, p_pins, p_many_bone_ik, p_parent, p_root_bone, p_tip_bone))); -} - -Ref IKConstraintBoneSegment3D::_create_next_bone(BoneId p_bone_id, Ref p_current_tip, Vector> &p_pins, ConstraintIK3D *p_many_bone_ik) { - String bone_name = skeleton->get_bone_name(p_bone_id); - Ref next_bone = Ref(memnew(IKConstraintBone3D(bone_name, skeleton, p_current_tip, p_pins, p_many_bone_ik->get_default_damp(), p_many_bone_ik))); - root_segment->bone_map[p_bone_id] = next_bone; - - return next_bone; -} - -void IKConstraintBoneSegment3D::_finalize_segment(Ref p_current_tip) { - tip = p_current_tip; - - if (tip->is_pinned()) { - _enable_pinned_descendants(); - } - - StringBuilder name_builder; - name_builder.append("IKBoneSegment"); - name_builder.append(root->get_name()); - name_builder.append("Root"); - name_builder.append(tip->get_name()); - name_builder.append("Tip"); - - String ik_bone_name = name_builder.as_string(); - set_name(ik_bone_name); - bones.clear(); - create_bone_list(bones, false); -} diff --git a/modules/constraint_ik/src/ik_bone_segment_3d.h b/modules/constraint_ik/src/ik_bone_segment_3d.h deleted file mode 100644 index 19512e0332dc..000000000000 --- a/modules/constraint_ik/src/ik_bone_segment_3d.h +++ /dev/null @@ -1,105 +0,0 @@ -/**************************************************************************/ -/* ik_bone_segment_3d.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef IK_CONSTRAINT_BONE_SEGMENT_3D_H -#define IK_CONSTRAINT_BONE_SEGMENT_3D_H - -#include "ik_bone_3d.h" -#include "ik_effector_3d.h" -#include "ik_effector_template_3d.h" -#include "math/qcp.h" -#include "scene/3d/skeleton_3d.h" - -#include "core/io/resource.h" -#include "core/object/ref_counted.h" - -class IKConstraintEffector3D; -class IKConstraintBone3D; -class IKLimitCone3D; - -class IKConstraintBoneSegment3D : public Resource { - GDCLASS(IKConstraintBoneSegment3D, Resource); - Ref root; - Ref tip; - Vector> bones; - Vector> pinned_bones; - Vector> child_segments; // Contains only direct child chains that end with effectors or have child that end with effectors - Ref parent_segment; - Ref root_segment; - Vector> effector_list; - PackedVector3Array target_headings; - PackedVector3Array tip_headings; - PackedVector3Array tip_headings_uniform; - Vector heading_weights; - Skeleton3D *skeleton = nullptr; - bool pinned_descendants = false; - double previous_deviation = INFINITY; - int32_t default_stabilizing_pass_count = 0; // Move to the stabilizing pass to the ik solver. Set it free. - bool _has_pinned_descendants(); - void _enable_pinned_descendants(); - void _update_target_headings(Ref p_for_bone, Vector *r_weights, PackedVector3Array *r_htarget); - void _update_tip_headings(Ref p_for_bone, PackedVector3Array *r_heading_tip); - void _set_optimal_rotation(Ref p_for_bone, PackedVector3Array *r_htip, PackedVector3Array *r_heading_tip, Vector *r_weights, float p_dampening = -1, bool p_translate = false, bool p_constraint_mode = false, double current_iteration = 0, double total_iterations = 0); - void _qcp_solver(const Vector &p_damp, float p_default_damp, bool p_translate, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iterations); - void _update_optimal_rotation(Ref p_for_bone, double p_damp, bool p_translate, bool p_constraint_mode, int32_t current_iteration, int32_t total_iterations); - float _get_manual_msd(const PackedVector3Array &r_htip, const PackedVector3Array &r_htarget, const Vector &p_weights); - HashMap> bone_map; - bool _is_parent_of_tip(Ref p_current_tip, BoneId p_tip_bone); - bool _has_multiple_children_or_pinned(Vector &r_children, Ref p_current_tip); - void _process_children(Vector &r_children, Ref p_current_tip, Vector> &r_pins, BoneId p_root_bone, BoneId p_tip_bone, ConstraintIK3D *p_many_bone_ik); - Ref _create_child_segment(String &p_child_name, Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ConstraintIK3D *p_many_bone_ik, Ref &p_parent); - Ref _create_next_bone(BoneId p_bone_id, Ref p_current_tip, Vector> &p_pins, ConstraintIK3D *p_many_bone_ik); - void _finalize_segment(Ref p_current_tip); - -protected: - static void _bind_methods(); - -public: - const double evec_prec = static_cast(1E-6); - void update_pinned_list(Vector> &r_weights); - static Quaternion clamp_to_cos_half_angle(Quaternion p_quat, double p_cos_half_angle); - static void recursive_create_headings_arrays_for(Ref p_bone_segment); - void create_headings_arrays(); - void recursive_create_penalty_array(Ref p_bone_segment, Vector> &r_penalty_array, Vector> &r_pinned_bones, double p_falloff); - void segment_solver(const Vector &p_damp, float p_default_damp, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iteration); - Ref get_root() const; - Ref get_tip() const; - bool is_pinned() const; - Vector> get_child_segments() const; - void create_bone_list(Vector> &p_list, bool p_recursive = false) const; - Ref get_ik_bone(BoneId p_bone) const; - void generate_default_segments(Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ConstraintIK3D *p_many_bone_ik); - IKConstraintBoneSegment3D() {} - IKConstraintBoneSegment3D(Skeleton3D *p_skeleton, StringName p_root_bone_name, Vector> &p_pins, ConstraintIK3D *p_many_bone_ik, const Ref &p_parent = nullptr, - BoneId root = -1, BoneId tip = -1, int32_t p_stabilizing_pass_count = 0); - ~IKConstraintBoneSegment3D() {} -}; - -#endif // IK_CONSTRAINT_BONE_SEGMENT_3D_H diff --git a/modules/constraint_ik/src/ik_effector_3d.cpp b/modules/constraint_ik/src/ik_effector_3d.cpp deleted file mode 100644 index c812b49c03c8..000000000000 --- a/modules/constraint_ik/src/ik_effector_3d.cpp +++ /dev/null @@ -1,145 +0,0 @@ -/**************************************************************************/ -/* ik_effector_3d.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "ik_effector_3d.h" - -#include "core/typedefs.h" -#include "ik_bone_3d.h" - -Ref IKConstraintEffector3D::get_ik_bone_3d() const { - return for_bone; -} - -bool IKConstraintEffector3D::is_following_translation_only() const { - return Math::is_zero_approx(direction_priorities.length_squared()); -} - -void IKConstraintEffector3D::set_direction_priorities(Vector3 p_direction_priorities) { - direction_priorities = p_direction_priorities; -} - -Vector3 IKConstraintEffector3D::get_direction_priorities() const { - return direction_priorities; -} - -Transform3D IKConstraintEffector3D::get_target_global_transform() const { - return target_relative_to_skeleton_origin; -} - -int32_t IKConstraintEffector3D::update_effector_target_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone, const Vector *p_weights) const { - ERR_FAIL_COND_V(p_index == -1, -1); - ERR_FAIL_NULL_V(p_headings, -1); - ERR_FAIL_COND_V(p_for_bone.is_null(), -1); - ERR_FAIL_NULL_V(p_weights, -1); - - int32_t index = p_index; - Vector3 bone_origin_relative_to_skeleton_origin = for_bone->get_bone_direction_global_pose().origin; - p_headings->write[index] = target_relative_to_skeleton_origin.origin - bone_origin_relative_to_skeleton_origin; - index++; - Vector3 priority = get_direction_priorities(); - for (int axis = Vector3::AXIS_X; axis <= Vector3::AXIS_Z; ++axis) { - if (priority[axis] > 0.0) { - real_t w = p_weights->get(index); - Vector3 column = target_relative_to_skeleton_origin.basis.get_column(axis); - - p_headings->write[index] = (column + target_relative_to_skeleton_origin.origin) - bone_origin_relative_to_skeleton_origin; - p_headings->write[index] *= Vector3(w, w, w); - index++; - p_headings->write[index] = (target_relative_to_skeleton_origin.origin - column) - bone_origin_relative_to_skeleton_origin; - p_headings->write[index] *= Vector3(w, w, w); - index++; - } - } - - return index; -} - -int32_t IKConstraintEffector3D::update_effector_tip_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone) const { - ERR_FAIL_COND_V(p_index == -1, -1); - ERR_FAIL_NULL_V(p_headings, -1); - ERR_FAIL_COND_V(p_for_bone.is_null(), -1); - - Transform3D tip_xform_relative_to_skeleton_origin = for_bone->get_bone_direction_global_pose(); - Basis tip_basis = tip_xform_relative_to_skeleton_origin.basis; - Vector3 bone_origin_relative_to_skeleton_origin = p_for_bone->get_bone_direction_global_pose().origin; - - int32_t index = p_index; - p_headings->write[index] = tip_xform_relative_to_skeleton_origin.origin - bone_origin_relative_to_skeleton_origin; - index++; - double distance = target_relative_to_skeleton_origin.origin.distance_to(bone_origin_relative_to_skeleton_origin); - double scale_by = MIN(distance, 1.0f); - const Vector3 priority = get_direction_priorities(); - - for (int axis = Vector3::AXIS_X; axis <= Vector3::AXIS_Z; ++axis) { - if (priority[axis] > 0.0) { - Vector3 column = tip_basis.get_column(axis) * priority[axis]; - - p_headings->write[index] = (column + tip_xform_relative_to_skeleton_origin.origin) - bone_origin_relative_to_skeleton_origin; - p_headings->write[index] *= scale_by; - index++; - - p_headings->write[index] = (tip_xform_relative_to_skeleton_origin.origin - column) - bone_origin_relative_to_skeleton_origin; - p_headings->write[index] *= scale_by; - index++; - } - } - - return index; -} - -void IKConstraintEffector3D::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_motion_propagation_factor", "amount"), - &IKConstraintEffector3D::set_motion_propagation_factor); - ClassDB::bind_method(D_METHOD("get_motion_propagation_factor"), - &IKConstraintEffector3D::get_motion_propagation_factor); - - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "motion_propagation_factor"), "set_motion_propagation_factor", "get_motion_propagation_factor"); -} - -void IKConstraintEffector3D::set_weight(real_t p_weight) { - weight = p_weight; -} - -real_t IKConstraintEffector3D::get_weight() const { - return weight; -} - -IKConstraintEffector3D::IKConstraintEffector3D(const Ref &p_current_bone) { - ERR_FAIL_COND(p_current_bone.is_null()); - for_bone = p_current_bone; -} - -void IKConstraintEffector3D::set_motion_propagation_factor(float p_motion_propagation_factor) { - motion_propagation_factor = CLAMP(p_motion_propagation_factor, 0.0, 1.0); -} - -float IKConstraintEffector3D::get_motion_propagation_factor() const { - return motion_propagation_factor; -} diff --git a/modules/constraint_ik/src/ik_effector_3d.h b/modules/constraint_ik/src/ik_effector_3d.h deleted file mode 100644 index 8cfab8baa80a..000000000000 --- a/modules/constraint_ik/src/ik_effector_3d.h +++ /dev/null @@ -1,85 +0,0 @@ -/**************************************************************************/ -/* ik_effector_3d.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef IK_CONSTRAINT_EFFECTOR_3D_H -#define IK_CONSTRAINT_EFFECTOR_3D_H - -#include "core/object/ref_counted.h" -#include "scene/3d/skeleton_3d.h" - -#define MIN_SCALE 0.1 - -class ConstraintIK3D; -class IKConstraintBone3D; - -class IKConstraintEffector3D : public Resource { - GDCLASS(IKConstraintEffector3D, Resource); - friend class IKConstraintBone3D; - friend class IKBoneSegment3D; - - Ref for_bone; - bool use_target_node_rotation = true; - NodePath target_node_path; - ObjectID target_node_cache; - Node *target_node_reference = nullptr; - bool target_static = false; - Transform3D target_transform; - - Transform3D target_relative_to_skeleton_origin; - int32_t num_headings = 7; - // See IKEffectorTemplate to change the defaults. - real_t weight = 0.0; - real_t motion_propagation_factor = 0.0; - PackedVector3Array target_headings; - PackedVector3Array tip_headings; - Vector heading_weights; - Vector3 direction_priorities; - -protected: - static void _bind_methods(); - -public: - IKConstraintEffector3D() = default; - void set_weight(real_t p_weight); - real_t get_weight() const; - void set_direction_priorities(Vector3 p_direction_priorities); - Vector3 get_direction_priorities() const; - const float MAX_KUSUDAMA_OPEN_CONES = 30; - float get_motion_propagation_factor() const; - void set_motion_propagation_factor(float p_motion_propagation_factor); - Transform3D get_target_global_transform() const; - Ref get_ik_bone_3d() const; - bool is_following_translation_only() const; - int32_t update_effector_target_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone, const Vector *p_weights) const; - int32_t update_effector_tip_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone) const; - IKConstraintEffector3D(const Ref &p_current_bone); -}; - -#endif // IK_CONSTRAINT_EFFECTOR_3D_H diff --git a/modules/constraint_ik/src/ik_effector_template_3d.cpp b/modules/constraint_ik/src/ik_effector_template_3d.cpp deleted file mode 100644 index e113dfe03e0a..000000000000 --- a/modules/constraint_ik/src/ik_effector_template_3d.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************/ -/* ik_effector_template_3d.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "ik_effector_template_3d.h" - -#include "many_bone_ik_3d.h" - -void IKConstraintEffectorTemplate3D::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_root_bone"), &IKConstraintEffectorTemplate3D::get_root_bone); - ClassDB::bind_method(D_METHOD("set_root_bone", "target_node"), &IKConstraintEffectorTemplate3D::set_root_bone); - - ClassDB::bind_method(D_METHOD("get_motion_propagation_factor"), &IKConstraintEffectorTemplate3D::get_motion_propagation_factor); - ClassDB::bind_method(D_METHOD("set_motion_propagation_factor", "motion_propagation_factor"), &IKConstraintEffectorTemplate3D::set_motion_propagation_factor); - - ClassDB::bind_method(D_METHOD("get_weight"), &IKConstraintEffectorTemplate3D::get_weight); - ClassDB::bind_method(D_METHOD("set_weight", "weight"), &IKConstraintEffectorTemplate3D::set_weight); - - ClassDB::bind_method(D_METHOD("get_direction_priorities"), &IKConstraintEffectorTemplate3D::get_direction_priorities); - ClassDB::bind_method(D_METHOD("set_direction_priorities", "direction_priorities"), &IKConstraintEffectorTemplate3D::set_direction_priorities); - - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "motion_propagation_factor"), "set_motion_propagation_factor", "get_motion_propagation_factor"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "weight"), "set_weight", "get_weight"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "direction_priorities"), "set_direction_priorities", "get_direction_priorities"); -} - -float IKConstraintEffectorTemplate3D::get_motion_propagation_factor() const { - return motion_propagation_factor; -} - -void IKConstraintEffectorTemplate3D::set_motion_propagation_factor(float p_motion_propagation_factor) { - motion_propagation_factor = p_motion_propagation_factor; -} - -IKConstraintEffectorTemplate3D::IKConstraintEffectorTemplate3D() { -} - -String IKConstraintEffectorTemplate3D::get_root_bone() const { - return root_bone; -} - -void IKConstraintEffectorTemplate3D::set_root_bone(String p_node_path) { - root_bone = p_node_path; -} diff --git a/modules/constraint_ik/src/ik_effector_template_3d.h b/modules/constraint_ik/src/ik_effector_template_3d.h deleted file mode 100644 index 009cf83d5179..000000000000 --- a/modules/constraint_ik/src/ik_effector_template_3d.h +++ /dev/null @@ -1,62 +0,0 @@ -/**************************************************************************/ -/* ik_effector_template_3d.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef IK_CONSTRAINT_EFFECTOR_TEMPLATE_3D_H -#define IK_CONSTRAINT_EFFECTOR_TEMPLATE_3D_H - -#include "core/io/resource.h" -#include "core/string/node_path.h" - -class IKConstraintEffectorTemplate3D : public Resource { - GDCLASS(IKConstraintEffectorTemplate3D, Resource); - - StringName root_bone; - NodePath target_node; - bool target_static = true; - real_t motion_propagation_factor = 1.0f; - real_t weight = 1.0f; - Vector3 priority_direction = Vector3(0.2f, 0.0f, 0.2f); // Purported ideal values are 1.0 / 3.0 for one direction, 1.0 / 5.0 for two directions and 1.0 / 7.0 for three directions. -protected: - static void _bind_methods(); - -public: - String get_root_bone() const; - void set_root_bone(String p_root_bone); - float get_motion_propagation_factor() const; - void set_motion_propagation_factor(float p_motion_propagation_factor); - real_t get_weight() const { return weight; } - void set_weight(real_t p_weight) { weight = p_weight; } - Vector3 get_direction_priorities() const { return priority_direction; } - void set_direction_priorities(Vector3 p_priority_direction) { priority_direction = p_priority_direction; } - - IKConstraintEffectorTemplate3D(); -}; - -#endif // IK_CONSTRAINT_EFFECTOR_TEMPLATE_3D_H diff --git a/modules/constraint_ik/src/many_bone_ik_3d.cpp b/modules/constraint_ik/src/many_bone_ik_3d.cpp deleted file mode 100644 index a146e8920974..000000000000 --- a/modules/constraint_ik/src/many_bone_ik_3d.cpp +++ /dev/null @@ -1,793 +0,0 @@ -/**************************************************************************/ -/* many_bone_ik_3d.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "many_bone_ik_3d.h" -#include "core/error/error_macros.h" -#include "core/math/math_defs.h" -#include "core/object/class_db.h" -#include "core/object/object.h" -#include "core/string/string_name.h" -#include "ik_bone_3d.h" -#include "ik_kusudama_3d.h" -#include "ik_open_cone_3d.h" -#include "scene/3d/skeleton_3d.h" -#include "scene/main/node.h" -#include "scene/main/scene_tree.h" - -void ConstraintIK3D::_update_ik_bones_transform() { - for (int32_t bone_i = bone_list.size(); bone_i-- > 0;) { - Ref bone = bone_list[bone_i]; - if (bone.is_null()) { - continue; - } - bone->set_initial_pose(get_skeleton()); - } -} - -void ConstraintIK3D::_update_skeleton_bones_transform() { - for (int32_t bone_i = bone_list.size(); bone_i-- > 0;) { - Ref bone = bone_list[bone_i]; - if (bone.is_null()) { - continue; - } - if (bone->get_bone_id() == -1) { - continue; - } - bone->set_skeleton_bone_pose(get_skeleton()); - } - update_gizmos(); -} - -void ConstraintIK3D::_get_property_list(List *p_list) const { - const Vector> ik_bones = get_bone_list(); - uint32_t constraint_usage = PROPERTY_USAGE_DEFAULT; - p_list->push_back( - PropertyInfo(Variant::INT, "constraint_count", - PROPERTY_HINT_RANGE, "0,256,or_greater", constraint_usage | PROPERTY_USAGE_ARRAY | PROPERTY_USAGE_READ_ONLY, - "Kusudama Constraints,constraints/")); - RBSet existing_constraints; - for (int constraint_i = 0; constraint_i < get_constraint_count(); constraint_i++) { - PropertyInfo bone_name; - bone_name.type = Variant::STRING_NAME; - bone_name.usage = constraint_usage; - bone_name.name = "constraints/" + itos(constraint_i) + "/bone_name"; - if (get_skeleton()) { - String names; - for (int bone_i = 0; bone_i < get_skeleton()->get_bone_count(); bone_i++) { - String name = get_skeleton()->get_bone_name(bone_i); - if (existing_constraints.has(name)) { - continue; - } - name += ","; - names += name; - existing_constraints.insert(name); - } - bone_name.hint = PROPERTY_HINT_ENUM_SUGGESTION; - bone_name.hint_string = names; - } else { - bone_name.hint = PROPERTY_HINT_NONE; - bone_name.hint_string = ""; - } - p_list->push_back(bone_name); - p_list->push_back( - PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/twist_start", PROPERTY_HINT_RANGE, "-359.9,359.9,0.1,radians,exp", constraint_usage)); - p_list->push_back( - PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/twist_end", PROPERTY_HINT_RANGE, "-359.9,359.9,0.1,radians,exp", constraint_usage)); - p_list->push_back( - PropertyInfo(Variant::INT, "constraints/" + itos(constraint_i) + "/kusudama_open_cone_count", PROPERTY_HINT_RANGE, "0,10,1", constraint_usage | PROPERTY_USAGE_ARRAY | PROPERTY_USAGE_READ_ONLY, - "Limit Cones,constraints/" + itos(constraint_i) + "/kusudama_open_cone/")); - for (int cone_i = 0; cone_i < get_kusudama_open_cone_count(constraint_i); cone_i++) { - p_list->push_back( - PropertyInfo(Variant::VECTOR3, "constraints/" + itos(constraint_i) + "/kusudama_open_cone/" + itos(cone_i) + "/center", PROPERTY_HINT_RANGE, "-1,1,0.1,exp", constraint_usage)); - - p_list->push_back( - PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/kusudama_open_cone/" + itos(cone_i) + "/radius", PROPERTY_HINT_RANGE, "0,180,0.1,radians,exp", constraint_usage)); - } - p_list->push_back( - PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/kusudama_twist", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); - p_list->push_back( - PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/kusudama_orientation", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); - p_list->push_back( - PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/bone_direction", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); - } -} - -bool ConstraintIK3D::_get(const StringName &p_name, Variant &r_ret) const { - String name = p_name; - if (name == "constraint_count") { - r_ret = get_constraint_count(); - return true; - } else if (name == "bone_count") { - r_ret = get_bone_count(); - return true; - } else if (name.begins_with("constraints/")) { - int index = name.get_slicec('/', 1).to_int(); - String what = name.get_slicec('/', 2); - ERR_FAIL_INDEX_V(index, constraint_count, false); - String begins = "constraints/" + itos(index) + "/kusudama_open_cone"; - if (what == "bone_name") { - ERR_FAIL_INDEX_V(index, constraint_names.size(), false); - r_ret = constraint_names[index]; - return true; - } else if (what == "twist_start") { - r_ret = get_joint_twist(index).x; - return true; - } else if (what == "twist_end") { - r_ret = get_joint_twist(index).y; - return true; - } else if (what == "kusudama_open_cone_count") { - r_ret = get_kusudama_open_cone_count(index); - return true; - } else if (name.begins_with(begins)) { - int32_t cone_index = name.get_slicec('/', 3).to_int(); - String cone_what = name.get_slicec('/', 4); - if (cone_what == "center") { - Vector3 center = get_kusudama_open_cone_center(index, cone_index); - r_ret = center; - return true; - } else if (cone_what == "radius") { - r_ret = get_kusudama_open_cone_radius(index, cone_index); - return true; - } - } else if (what == "bone_direction") { - r_ret = get_direction_transform_of_bone(index); - return true; - } else if (what == "kusudama_orientation") { - r_ret = get_orientation_transform_of_constraint(index); - return true; - } else if (what == "kusudama_twist") { - r_ret = get_twist_transform_of_constraint(index); - return true; - } - } - return false; -} - -bool ConstraintIK3D::_set(const StringName &p_name, const Variant &p_value) { - String name = p_name; - if (name == "constraint_count") { - _set_constraint_count(p_value); - return true; - } else if (name.begins_with("constraints/")) { - int index = name.get_slicec('/', 1).to_int(); - String what = name.get_slicec('/', 2); - String begins = "constraints/" + itos(index) + "/kusudama_open_cone/"; - if (index >= constraint_names.size()) { - _set_constraint_count(constraint_count); - } - if (what == "bone_name") { - set_constraint_name_at_index(index, p_value); - return true; - } else if (what == "twist_from") { - Vector2 twist_from = get_joint_twist(index); - set_joint_twist(index, Vector2(p_value, twist_from.y)); - return true; - } else if (what == "twist_range") { - Vector2 twist_range = get_joint_twist(index); - set_joint_twist(index, Vector2(twist_range.x, p_value)); - return true; - } else if (what == "kusudama_open_cone_count") { - set_kusudama_open_cone_count(index, p_value); - return true; - } else if (name.begins_with(begins)) { - int cone_index = name.get_slicec('/', 3).to_int(); - String cone_what = name.get_slicec('/', 4); - if (cone_what == "center") { - set_kusudama_open_cone_center(index, cone_index, p_value); - return true; - } else if (cone_what == "radius") { - set_kusudama_open_cone_radius(index, cone_index, p_value); - return true; - } - } else if (what == "bone_direction") { - set_direction_transform_of_bone(index, p_value); - return true; - } else if (what == "kusudama_orientation") { - set_orientation_transform_of_constraint(index, p_value); - return true; - } else if (what == "kusudama_twist") { - set_twist_transform_of_constraint(index, p_value); - return true; - } - } - - return false; -} - -void ConstraintIK3D::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_constraint_name_at_index", "index", "name"), &ConstraintIK3D::set_constraint_name_at_index); - ClassDB::bind_method(D_METHOD("get_twist_transform_of_constraint", "index"), &ConstraintIK3D::get_twist_transform_of_constraint); - ClassDB::bind_method(D_METHOD("set_twist_transform_of_constraint", "index", "transform"), &ConstraintIK3D::set_twist_transform_of_constraint); - ClassDB::bind_method(D_METHOD("get_orientation_transform_of_constraint", "index"), &ConstraintIK3D::get_orientation_transform_of_constraint); - ClassDB::bind_method(D_METHOD("set_orientation_transform_of_constraint", "index", "transform"), &ConstraintIK3D::set_orientation_transform_of_constraint); - ClassDB::bind_method(D_METHOD("get_direction_transform_of_bone", "index"), &ConstraintIK3D::get_direction_transform_of_bone); - ClassDB::bind_method(D_METHOD("set_direction_transform_of_bone", "index", "transform"), &ConstraintIK3D::set_direction_transform_of_bone); - ClassDB::bind_method(D_METHOD("register_skeleton"), &ConstraintIK3D::register_skeleton); - ClassDB::bind_method(D_METHOD("reset_constraints"), &ConstraintIK3D::reset_constraints); - ClassDB::bind_method(D_METHOD("set_dirty"), &ConstraintIK3D::set_dirty); - ClassDB::bind_method(D_METHOD("set_kusudama_open_cone_radius", "index", "cone_index", "radius"), &ConstraintIK3D::set_kusudama_open_cone_radius); - ClassDB::bind_method(D_METHOD("get_kusudama_open_cone_radius", "index", "cone_index"), &ConstraintIK3D::get_kusudama_open_cone_radius); - ClassDB::bind_method(D_METHOD("set_kusudama_open_cone_center", "index", "cone_index", "center"), &ConstraintIK3D::set_kusudama_open_cone_center); - ClassDB::bind_method(D_METHOD("get_kusudama_open_cone_center", "index", "cone_index"), &ConstraintIK3D::get_kusudama_open_cone_center); - ClassDB::bind_method(D_METHOD("set_kusudama_open_cone_count", "index", "count"), &ConstraintIK3D::set_kusudama_open_cone_count); - ClassDB::bind_method(D_METHOD("get_kusudama_open_cone_count", "index"), &ConstraintIK3D::get_kusudama_open_cone_count); - ClassDB::bind_method(D_METHOD("set_joint_twist", "index", "limit"), &ConstraintIK3D::set_joint_twist); - ClassDB::bind_method(D_METHOD("get_joint_twist", "index"), &ConstraintIK3D::get_joint_twist); - - ClassDB::bind_method(D_METHOD("get_constraint_name", "index"), &ConstraintIK3D::get_constraint_name); - ClassDB::bind_method(D_METHOD("get_iterations_per_frame"), &ConstraintIK3D::get_iterations_per_frame); - ClassDB::bind_method(D_METHOD("set_iterations_per_frame", "count"), &ConstraintIK3D::set_iterations_per_frame); - ClassDB::bind_method(D_METHOD("find_constraint", "name"), &ConstraintIK3D::find_constraint); - ClassDB::bind_method(D_METHOD("get_constraint_count"), &ConstraintIK3D::get_constraint_count); - ClassDB::bind_method(D_METHOD("set_constraint_count", "count"), &ConstraintIK3D::_set_constraint_count); - ClassDB::bind_method(D_METHOD("get_default_damp"), &ConstraintIK3D::get_default_damp); - ClassDB::bind_method(D_METHOD("set_default_damp", "damp"), &ConstraintIK3D::set_default_damp); - ClassDB::bind_method(D_METHOD("get_bone_count"), &ConstraintIK3D::get_bone_count); - ClassDB::bind_method(D_METHOD("set_constraint_mode", "enabled"), &ConstraintIK3D::set_constraint_mode); - ClassDB::bind_method(D_METHOD("get_constraint_mode"), &ConstraintIK3D::get_constraint_mode); - ClassDB::bind_method(D_METHOD("set_ui_selected_bone", "bone"), &ConstraintIK3D::set_ui_selected_bone); - ClassDB::bind_method(D_METHOD("get_ui_selected_bone"), &ConstraintIK3D::get_ui_selected_bone); - ClassDB::bind_method(D_METHOD("set_stabilization_passes", "passes"), &ConstraintIK3D::set_stabilization_passes); - ClassDB::bind_method(D_METHOD("get_stabilization_passes"), &ConstraintIK3D::get_stabilization_passes); - - ADD_PROPERTY(PropertyInfo(Variant::INT, "iterations_per_frame", PROPERTY_HINT_RANGE, "1,150,1,or_greater"), "set_iterations_per_frame", "get_iterations_per_frame"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "default_damp", PROPERTY_HINT_RANGE, "0.01,180.0,0.1,radians,exp", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_UPDATE_ALL_IF_MODIFIED), "set_default_damp", "get_default_damp"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "constraint_mode"), "set_constraint_mode", "get_constraint_mode"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "ui_selected_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_ui_selected_bone", "get_ui_selected_bone"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "stabilization_passes"), "set_stabilization_passes", "get_stabilization_passes"); -} - -ConstraintIK3D::ConstraintIK3D() { -} - -ConstraintIK3D::~ConstraintIK3D() { -} - -void ConstraintIK3D::_set_constraint_count(int32_t p_count) { - int32_t old_count = constraint_names.size(); - constraint_count = p_count; - constraint_names.resize(p_count); - joint_twist.resize(p_count); - kusudama_open_cone_count.resize(p_count); - kusudama_open_cones.resize(p_count); - for (int32_t constraint_i = p_count; constraint_i-- > old_count;) { - constraint_names.write[constraint_i] = String(); - kusudama_open_cone_count.write[constraint_i] = 0; - kusudama_open_cones.write[constraint_i].resize(1); - kusudama_open_cones.write[constraint_i].write[0] = Vector4(0, 1, 0, 0.01745f); - joint_twist.write[constraint_i] = Vector2(0, 0.01745f); - } - set_dirty(); - notify_property_list_changed(); -} - -int32_t ConstraintIK3D::get_constraint_count() const { - return constraint_count; -} - -inline StringName ConstraintIK3D::get_constraint_name(int32_t p_index) const { - ERR_FAIL_INDEX_V(p_index, constraint_names.size(), StringName()); - return constraint_names[p_index]; -} - -Vector2 ConstraintIK3D::get_joint_twist(int32_t p_index) const { - ERR_FAIL_INDEX_V(p_index, joint_twist.size(), Vector2()); - return joint_twist[p_index]; -} - -void ConstraintIK3D::set_joint_twist(int32_t p_index, Vector2 p_to) { - ERR_FAIL_INDEX(p_index, constraint_count); - joint_twist.write[p_index] = p_to; - set_dirty(); -} - -void ConstraintIK3D::set_kusudama_open_cone(int32_t p_constraint_index, int32_t p_index, - Vector3 p_center, float p_radius) { - ERR_FAIL_INDEX(p_constraint_index, kusudama_open_cones.size()); - Vector cones = kusudama_open_cones.write[p_constraint_index]; - if (Math::is_zero_approx(p_center.length_squared())) { - p_center = Vector3(0.0f, 1.0f, 0.0f); - } - Vector3 center = p_center.normalized(); - Vector4 cone; - cone.x = center.x; - cone.y = center.y; - cone.z = center.z; - cone.w = p_radius; - cones.write[p_index] = cone; - kusudama_open_cones.write[p_constraint_index] = cones; - set_dirty(); -} - -float ConstraintIK3D::get_kusudama_open_cone_radius(int32_t p_constraint_index, int32_t p_index) const { - ERR_FAIL_INDEX_V(p_constraint_index, kusudama_open_cones.size(), Math_TAU); - ERR_FAIL_INDEX_V(p_index, kusudama_open_cones[p_constraint_index].size(), Math_TAU); - return kusudama_open_cones[p_constraint_index][p_index].w; -} - -int32_t ConstraintIK3D::get_kusudama_open_cone_count(int32_t p_constraint_index) const { - ERR_FAIL_INDEX_V(p_constraint_index, kusudama_open_cone_count.size(), 0); - return kusudama_open_cone_count[p_constraint_index]; -} - -void ConstraintIK3D::set_kusudama_open_cone_count(int32_t p_constraint_index, int32_t p_count) { - ERR_FAIL_INDEX(p_constraint_index, kusudama_open_cone_count.size()); - ERR_FAIL_INDEX(p_constraint_index, kusudama_open_cones.size()); - int32_t old_cone_count = kusudama_open_cones[p_constraint_index].size(); - kusudama_open_cone_count.write[p_constraint_index] = p_count; - Vector &cones = kusudama_open_cones.write[p_constraint_index]; - cones.resize(p_count); - String bone_name = get_constraint_name(p_constraint_index); - Transform3D bone_transform = get_direction_transform_of_bone(p_constraint_index); - Vector3 forward_axis = -bone_transform.basis.get_column(Vector3::AXIS_Y).normalized(); - for (int32_t cone_i = p_count; cone_i-- > old_cone_count;) { - Vector4 &cone = cones.write[cone_i]; - cone.x = forward_axis.x; - cone.y = forward_axis.y; - cone.z = forward_axis.z; - cone.w = Math::deg_to_rad(0.0f); - } - set_dirty(); - notify_property_list_changed(); -} - -real_t ConstraintIK3D::get_default_damp() const { - return default_damp; -} - -void ConstraintIK3D::set_default_damp(float p_default_damp) { - default_damp = p_default_damp; - set_dirty(); -} - -void ConstraintIK3D::set_kusudama_open_cone_radius(int32_t p_effector_index, int32_t p_index, float p_radius) { - ERR_FAIL_INDEX(p_effector_index, kusudama_open_cone_count.size()); - ERR_FAIL_INDEX(p_effector_index, kusudama_open_cones.size()); - ERR_FAIL_INDEX(p_index, kusudama_open_cone_count[p_effector_index]); - ERR_FAIL_INDEX(p_index, kusudama_open_cones[p_effector_index].size()); - Vector4 &cone = kusudama_open_cones.write[p_effector_index].write[p_index]; - cone.w = p_radius; - set_dirty(); -} - -void ConstraintIK3D::set_kusudama_open_cone_center(int32_t p_effector_index, int32_t p_index, Vector3 p_center) { - ERR_FAIL_INDEX(p_effector_index, kusudama_open_cones.size()); - ERR_FAIL_INDEX(p_index, kusudama_open_cones[p_effector_index].size()); - Vector4 &cone = kusudama_open_cones.write[p_effector_index].write[p_index]; - if (Math::is_zero_approx(p_center.length_squared())) { - cone.x = 0; - cone.y = 1; - cone.z = 0; - } else { - cone.x = p_center.x; - cone.y = p_center.y; - cone.z = p_center.z; - } - set_dirty(); -} - -Vector3 ConstraintIK3D::get_kusudama_open_cone_center(int32_t p_constraint_index, int32_t p_index) const { - if (unlikely((p_constraint_index) < 0 || (p_constraint_index) >= (kusudama_open_cones.size()))) { - ERR_PRINT_ONCE("Can't get limit cone center."); - return Vector3(0.0, 0.0, 1.0); - } - if (unlikely((p_index) < 0 || (p_index) >= (kusudama_open_cones[p_constraint_index].size()))) { - ERR_PRINT_ONCE("Can't get limit cone center."); - return Vector3(0.0, 0.0, 1.0); - } - const Vector4 &cone = kusudama_open_cones[p_constraint_index][p_index]; - Vector3 ret; - ret.x = cone.x; - ret.y = cone.y; - ret.z = cone.z; - return ret; -} - -void ConstraintIK3D::set_constraint_name_at_index(int32_t p_index, String p_name) { - ERR_FAIL_INDEX(p_index, constraint_names.size()); - constraint_names.write[p_index] = p_name; - set_dirty(); -} - -Vector> ConstraintIK3D::get_segmented_skeletons() { - return segmented_skeletons; -} - -float ConstraintIK3D::get_iterations_per_frame() const { - return iterations_per_frame; -} - -void ConstraintIK3D::set_iterations_per_frame(const float &p_iterations_per_frame) { - iterations_per_frame = p_iterations_per_frame; -} - -void ConstraintIK3D::_process_modification() { - if (!get_skeleton()) { - return; - } - if (!segmented_skeletons.size()) { - set_dirty(); - } - if (is_dirty) { - is_dirty = false; - _bone_list_changed(); - } - if (bone_list.size()) { - Ref root_ik_bone = bone_list.write[0]->get_ik_transform(); - if (root_ik_bone.is_null()) { - return; - } - Skeleton3D *skeleton = get_skeleton(); - godot_skeleton_transform.instantiate(); - godot_skeleton_transform->set_transform(skeleton->get_transform()); - godot_skeleton_transform_inverse = skeleton->get_transform().affine_inverse(); - } - if (!is_enabled()) { - return; - } - if (!is_visible()) { - return; - } - for (int32_t i = 0; i < get_iterations_per_frame(); i++) { - for (Ref segmented_skeleton : segmented_skeletons) { - if (segmented_skeleton.is_null()) { - continue; - } - segmented_skeleton->segment_solver(bone_damp, get_default_damp(), get_constraint_mode(), i, get_iterations_per_frame()); - } - } - _update_skeleton_bones_transform(); -} - -void ConstraintIK3D::set_dirty() { - is_dirty = true; -} - -int32_t ConstraintIK3D::find_constraint(String p_string) const { - for (int32_t constraint_i = 0; constraint_i < constraint_count; constraint_i++) { - if (get_constraint_name(constraint_i) == p_string) { - return constraint_i; - } - } - return -1; -} - -void ConstraintIK3D::_set_bone_count(int32_t p_count) { - bone_damp.resize(p_count); - for (int32_t bone_i = p_count; bone_i-- > bone_count;) { - bone_damp.write[bone_i] = get_default_damp(); - } - bone_count = p_count; - set_dirty(); - notify_property_list_changed(); -} - -int32_t ConstraintIK3D::get_bone_count() const { - return bone_count; -} - -Vector> ConstraintIK3D::get_bone_list() const { - return bone_list; -} - -void ConstraintIK3D::set_direction_transform_of_bone(int32_t p_index, Transform3D p_transform) { - ERR_FAIL_INDEX(p_index, constraint_names.size()); - if (!get_skeleton()) { - return; - } - String bone_name = constraint_names[p_index]; - int32_t bone_index = get_skeleton()->find_bone(bone_name); - for (Ref segmented_skeleton : segmented_skeletons) { - if (segmented_skeleton.is_null()) { - continue; - } - Ref ik_bone = segmented_skeleton->get_ik_bone(bone_index); - if (ik_bone.is_null() || ik_bone->get_constraint().is_null()) { - continue; - } - if (ik_bone->get_bone_direction_transform().is_null()) { - continue; - } - ik_bone->get_bone_direction_transform()->set_transform(p_transform); - break; - } -} - -Transform3D ConstraintIK3D::get_direction_transform_of_bone(int32_t p_index) const { - if (p_index < 0 || p_index >= constraint_names.size() || get_skeleton() == nullptr) { - return Transform3D(); - } - - String bone_name = constraint_names[p_index]; - int32_t bone_index = get_skeleton()->find_bone(bone_name); - for (const Ref &segmented_skeleton : segmented_skeletons) { - if (segmented_skeleton.is_null()) { - continue; - } - Ref ik_bone = segmented_skeleton->get_ik_bone(bone_index); - if (ik_bone.is_null() || ik_bone->get_constraint().is_null()) { - continue; - } - return ik_bone->get_bone_direction_transform()->get_transform(); - } - return Transform3D(); -} - -Transform3D ConstraintIK3D::get_orientation_transform_of_constraint(int32_t p_index) const { - ERR_FAIL_INDEX_V(p_index, constraint_names.size(), Transform3D()); - String bone_name = constraint_names[p_index]; - if (!segmented_skeletons.size()) { - return Transform3D(); - } - if (!get_skeleton()) { - return Transform3D(); - } - for (Ref segmented_skeleton : segmented_skeletons) { - if (segmented_skeleton.is_null()) { - continue; - } - Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); - if (ik_bone.is_null()) { - continue; - } - if (ik_bone->get_constraint().is_null()) { - continue; - } - return ik_bone->get_constraint_orientation_transform()->get_transform(); - } - return Transform3D(); -} - -void ConstraintIK3D::set_orientation_transform_of_constraint(int32_t p_index, Transform3D p_transform) { - ERR_FAIL_INDEX(p_index, constraint_names.size()); - String bone_name = constraint_names[p_index]; - if (!get_skeleton()) { - return; - } - for (Ref segmented_skeleton : segmented_skeletons) { - if (segmented_skeleton.is_null()) { - continue; - } - Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); - if (ik_bone.is_null()) { - continue; - } - if (ik_bone->get_constraint().is_null()) { - continue; - } - ik_bone->get_constraint_orientation_transform()->set_transform(p_transform); - break; - } -} - -Transform3D ConstraintIK3D::get_twist_transform_of_constraint(int32_t p_index) const { - ERR_FAIL_INDEX_V(p_index, constraint_names.size(), Transform3D()); - String bone_name = constraint_names[p_index]; - if (!segmented_skeletons.size()) { - return Transform3D(); - } - if (!get_skeleton()) { - return Transform3D(); - } - for (Ref segmented_skeleton : segmented_skeletons) { - if (segmented_skeleton.is_null()) { - continue; - } - Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); - if (ik_bone.is_null()) { - continue; - } - if (ik_bone->get_constraint().is_null()) { - continue; - } - return ik_bone->get_constraint_twist_transform()->get_transform(); - } - return Transform3D(); -} - -void ConstraintIK3D::set_twist_transform_of_constraint(int32_t p_index, Transform3D p_transform) { - ERR_FAIL_INDEX(p_index, constraint_names.size()); - String bone_name = constraint_names[p_index]; - if (!get_skeleton()) { - return; - } - for (Ref segmented_skeleton : segmented_skeletons) { - if (segmented_skeleton.is_null()) { - continue; - } - Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); - if (ik_bone.is_null()) { - continue; - } - if (ik_bone->get_constraint().is_null()) { - continue; - } - ik_bone->get_constraint_twist_transform()->set_transform(p_transform); - break; - } -} - -void ConstraintIK3D::register_skeleton() { - if (!get_constraint_count()) { - reset_constraints(); - } - set_dirty(); -} - -void ConstraintIK3D::reset_constraints() { - Skeleton3D *skeleton = get_skeleton(); - if (skeleton) { - int32_t saved_constraint_count = constraint_names.size(); - _set_constraint_count(0); - _set_constraint_count(saved_constraint_count); - _set_bone_count(0); - _set_bone_count(saved_constraint_count); - } - set_dirty(); -} - -bool ConstraintIK3D::get_constraint_mode() const { - return is_constraint_mode; -} - -void ConstraintIK3D::set_constraint_mode(bool p_enabled) { - is_constraint_mode = p_enabled; -} - -int32_t ConstraintIK3D::get_ui_selected_bone() const { - return ui_selected_bone; -} - -void ConstraintIK3D::set_ui_selected_bone(int32_t p_ui_selected_bone) { - ui_selected_bone = p_ui_selected_bone; -} - -void ConstraintIK3D::set_stabilization_passes(int32_t p_passes) { - stabilize_passes = p_passes; - set_dirty(); -} - -int32_t ConstraintIK3D::get_stabilization_passes() { - return stabilize_passes; -} - -Transform3D ConstraintIK3D::get_godot_skeleton_transform_inverse() { - return godot_skeleton_transform_inverse; -} - -Ref ConstraintIK3D::get_godot_skeleton_transform() { - return godot_skeleton_transform; -} - -void ConstraintIK3D::add_constraint() { - int32_t old_count = constraint_count; - _set_constraint_count(constraint_count + 1); - constraint_names.write[old_count] = String(); - kusudama_open_cone_count.write[old_count] = 0; - kusudama_open_cones.write[old_count].resize(1); - kusudama_open_cones.write[old_count].write[0] = Vector4(0, 1, 0, Math_PI); - joint_twist.write[old_count] = Vector2(0, Math_PI); - set_dirty(); -} - -void ConstraintIK3D::_bone_list_changed() { - Skeleton3D *skeleton = get_skeleton(); - Vector roots = skeleton->get_parentless_bones(); - if (roots.is_empty()) { - return; - } - bone_list.clear(); - segmented_skeletons.clear(); - for (BoneId root_bone_index : roots) { - String parentless_bone = skeleton->get_bone_name(root_bone_index); - Vector> pins; - Array bone_queue; - bone_queue.push_back(root_bone_index); - - while (!bone_queue.is_empty()) { - BoneId current_bone = bone_queue.front(); - bone_queue.pop_front(); - Vector children = skeleton->get_bone_children(current_bone); - for (BoneId child_bone : children) { - bone_queue.push_back(child_bone); - } - Ref effector = memnew(IKConstraintEffectorTemplate3D); - effector->set_root_bone(skeleton->get_bone_name(root_bone_index)); - effector->set_motion_propagation_factor(1.0f); - effector->set_weight(1.0f); - effector->set_direction_priorities(Vector3(0.2f, 0.0f, 0.2f)); - pins.push_back(effector); - } - Ref segmented_skeleton = Ref(memnew(IKConstraintBoneSegment3D(skeleton, parentless_bone, pins, this, nullptr, root_bone_index, -1, stabilize_passes))); - ik_origin.instantiate(); - segmented_skeleton->get_root()->get_ik_transform()->set_parent(ik_origin); - segmented_skeleton->generate_default_segments(pins, root_bone_index, -1, this); - Vector> new_bone_list; - segmented_skeleton->create_bone_list(new_bone_list, true); - bone_list.append_array(new_bone_list); - Vector> weight_array; - segmented_skeleton->update_pinned_list(weight_array); - segmented_skeleton->recursive_create_headings_arrays_for(segmented_skeleton); - segmented_skeletons.push_back(segmented_skeleton); - } - _update_ik_bones_transform(); - for (Ref &ik_bone_3d : bone_list) { - ik_bone_3d->update_default_bone_direction_transform(skeleton); - } - for (int constraint_i = 0; constraint_i < constraint_count; ++constraint_i) { - String bone = constraint_names[constraint_i]; - BoneId bone_id = skeleton->find_bone(bone); - for (Ref &ik_bone_3d : bone_list) { - if (ik_bone_3d->get_bone_id() != bone_id) { - continue; - } - Ref constraint; - constraint.instantiate(); - constraint->enable_orientational_limits(); - - int32_t cone_count = kusudama_open_cone_count[constraint_i]; - const Vector &cones = kusudama_open_cones[constraint_i]; - for (int32_t cone_i = 0; cone_i < cone_count; ++cone_i) { - const Vector4 &cone = cones[cone_i]; - Ref new_cone; - new_cone.instantiate(); - new_cone->set_attached_to(constraint); - new_cone->set_radius(MAX(1.0e-38, cone.w)); - new_cone->set_control_point(Vector3(cone.x, cone.y, cone.z).normalized()); - constraint->add_open_cone(new_cone); - } - - const Vector2 axial_limit = get_joint_twist(constraint_i); - constraint->enable_axial_limits(); - constraint->set_axial_limits(axial_limit.x, axial_limit.y); - ik_bone_3d->add_constraint(constraint); - constraint->_update_constraint(ik_bone_3d->get_constraint_twist_transform()); - break; - } - } -} - -void ConstraintIK3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) { - if (p_old) { - if (p_old->is_connected(SNAME("bone_list_changed"), callable_mp(this, &ConstraintIK3D::_bone_list_changed))) { - p_old->disconnect(SNAME("bone_list_changed"), callable_mp(this, &ConstraintIK3D::_bone_list_changed)); - } - } - if (p_new) { - if (!p_new->is_connected(SNAME("bone_list_changed"), callable_mp(this, &ConstraintIK3D::_bone_list_changed))) { - p_new->connect(SNAME("bone_list_changed"), callable_mp(this, &ConstraintIK3D::_bone_list_changed)); - } - } - if (is_connected(SNAME("modification_processed"), callable_mp(this, &ConstraintIK3D::_update_ik_bones_transform))) { - disconnect(SNAME("modification_processed"), callable_mp(this, &ConstraintIK3D::_update_ik_bones_transform)); - } - connect(SNAME("modification_processed"), callable_mp(this, &ConstraintIK3D::_update_ik_bones_transform)); - _bone_list_changed(); -} diff --git a/modules/constraint_ik/src/many_bone_ik_3d.h b/modules/constraint_ik/src/many_bone_ik_3d.h deleted file mode 100644 index 00514542310e..000000000000 --- a/modules/constraint_ik/src/many_bone_ik_3d.h +++ /dev/null @@ -1,134 +0,0 @@ -/**************************************************************************/ -/* many_bone_ik_3d.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef MANY_BONE_IK_3D_H -#define MANY_BONE_IK_3D_H - -#include "core/math/math_defs.h" -#include "core/math/transform_3d.h" -#include "core/math/vector3.h" -#include "core/object/ref_counted.h" -#include "ik_bone_3d.h" -#include "ik_effector_template_3d.h" -#include "math/ik_node_3d.h" -#include "scene/3d/skeleton_3d.h" -#include "scene/3d/skeleton_modifier_3d.h" - -class ConstraintIK3DState; -class ConstraintIK3D : public SkeletonModifier3D { - GDCLASS(ConstraintIK3D, SkeletonModifier3D); - bool is_constraint_mode = false; - NodePath skeleton_path; - Vector> segmented_skeletons; - int32_t constraint_count = 0, pin_count = 0, bone_count = 0; - Vector constraint_names; - Vector> bone_list; - Vector joint_twist; - Vector bone_damp; - Vector> kusudama_open_cones; - Vector kusudama_open_cone_count; - float MAX_KUSUDAMA_OPEN_CONES = 10; - int32_t iterations_per_frame = 15; - float default_damp = Math::deg_to_rad(5.0f); - Ref godot_skeleton_transform; - Transform3D godot_skeleton_transform_inverse; - Ref ik_origin; - bool is_dirty = true; - NodePath skeleton_node_path = NodePath(".."); - int32_t ui_selected_bone = -1, stabilize_passes = 0; - - void _on_timer_timeout(); - void _update_ik_bones_transform(); - void _update_skeleton_bones_transform(); - void set_constraint_name_at_index(int32_t p_index, String p_name); - void _set_constraint_count(int32_t p_count); - void _set_bone_count(int32_t p_count); - void _set_pin_root_bone(int32_t p_pin_index, const String &p_root_bone); - String _get_pin_root_bone(int32_t p_pin_index) const; - void _bone_list_changed(); - void _pose_updated(); - void _update_ik_bone_pose(int32_t p_bone_idx); - -protected: - bool _set(const StringName &p_name, const Variant &p_value); - bool _get(const StringName &p_name, Variant &r_ret) const; - void _get_property_list(List *p_list) const; - static void _bind_methods(); - virtual void _process_modification() override; - void _skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) override; - -public: - void set_state(Ref p_state); - Ref get_state() const; - void add_constraint(); - void set_stabilization_passes(int32_t p_passes); - int32_t get_stabilization_passes(); - Transform3D get_godot_skeleton_transform_inverse(); - Ref get_godot_skeleton_transform(); - void set_ui_selected_bone(int32_t p_ui_selected_bone); - int32_t get_ui_selected_bone() const; - void set_constraint_mode(bool p_enabled); - bool get_constraint_mode() const; - void register_skeleton(); - void reset_constraints(); - Vector> get_bone_list() const; - Vector> get_segmented_skeletons(); - float get_iterations_per_frame() const; - void set_iterations_per_frame(const float &p_iterations_per_frame); - void queue_print_skeleton(); - real_t get_default_damp() const; - void set_default_damp(float p_default_damp); - int32_t find_constraint(String p_string) const; - int32_t get_constraint_count() const; - StringName get_constraint_name(int32_t p_index) const; - void set_twist_transform_of_constraint(int32_t p_index, Transform3D p_transform); - Transform3D get_twist_transform_of_constraint(int32_t p_index) const; - void set_orientation_transform_of_constraint(int32_t p_index, Transform3D p_transform); - Transform3D get_orientation_transform_of_constraint(int32_t p_index) const; - void set_direction_transform_of_bone(int32_t p_index, Transform3D p_transform); - Transform3D get_direction_transform_of_bone(int32_t p_index) const; - Vector2 get_joint_twist(int32_t p_index) const; - void set_joint_twist(int32_t p_index, Vector2 p_twist); - void set_kusudama_open_cone(int32_t p_bone, int32_t p_index, - Vector3 p_center, float p_radius); - Vector3 get_kusudama_open_cone_center(int32_t p_constraint_index, int32_t p_index) const; - float get_kusudama_open_cone_radius(int32_t p_constraint_index, int32_t p_index) const; - int32_t get_kusudama_open_cone_count(int32_t p_constraint_index) const; - int32_t get_bone_count() const; - void set_kusudama_twist_from_to(int32_t p_index, float from, float to); - void set_kusudama_open_cone_count(int32_t p_constraint_index, int32_t p_count); - void set_kusudama_open_cone_center(int32_t p_constraint_index, int32_t p_index, Vector3 p_center); - void set_kusudama_open_cone_radius(int32_t p_constraint_index, int32_t p_index, float p_radius); - ConstraintIK3D(); - ~ConstraintIK3D(); - void set_dirty(); -}; - -#endif // MANY_BONE_IK_3D_H diff --git a/modules/constraint_ik/src/math/ik_node_3d.cpp b/modules/constraint_ik/src/math/ik_node_3d.cpp deleted file mode 100644 index 66fa7606f224..000000000000 --- a/modules/constraint_ik/src/math/ik_node_3d.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/**************************************************************************/ -/* ik_node_3d.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "ik_node_3d.h" - -void IKConstraintNode3D::_propagate_transform_changed() { - Vector> to_remove; - - for (Ref transform : children) { - if (transform.is_null()) { - to_remove.push_back(transform); - } else { - transform->_propagate_transform_changed(); - } - } - - for (Ref transform : to_remove) { - children.erase(transform); - } - - dirty |= DIRTY_GLOBAL; -} - -void IKConstraintNode3D::_update_local_transform() const { - local_transform.basis = rotation.scaled(scale); - dirty &= ~DIRTY_LOCAL; -} - -void IKConstraintNode3D::rotate_local_with_global(const Basis &p_basis, bool p_propagate) { - if (parent.get_ref().is_null()) { - return; - } - Ref parent_ik_node = parent.get_ref(); - const Basis &new_rot = parent_ik_node->get_global_transform().basis; - local_transform.basis = new_rot.inverse() * p_basis * new_rot * local_transform.basis; - dirty |= DIRTY_GLOBAL; - if (p_propagate) { - _propagate_transform_changed(); - } -} - -void IKConstraintNode3D::set_transform(const Transform3D &p_transform) { - if (local_transform != p_transform) { - local_transform = p_transform; - dirty |= DIRTY_VECTORS; - _propagate_transform_changed(); - } -} - -void IKConstraintNode3D::set_global_transform(const Transform3D &p_transform) { - Ref ik_node = parent.get_ref(); - Transform3D xform = ik_node.is_valid() ? ik_node->get_global_transform().affine_inverse() * p_transform : p_transform; - local_transform = xform; - dirty |= DIRTY_VECTORS; - _propagate_transform_changed(); -} - -Transform3D IKConstraintNode3D::get_transform() const { - if (dirty & DIRTY_LOCAL) { - _update_local_transform(); - } - - return local_transform; -} - -Transform3D IKConstraintNode3D::get_global_transform() const { - if (dirty & DIRTY_GLOBAL) { - if (dirty & DIRTY_LOCAL) { - _update_local_transform(); - } - Ref ik_node = parent.get_ref(); - if (ik_node.is_valid()) { - global_transform = ik_node->get_global_transform() * local_transform; - } else { - global_transform = local_transform; - } - - if (disable_scale) { - global_transform.basis.orthogonalize(); - } - - dirty &= ~DIRTY_GLOBAL; - } - - return global_transform; -} - -void IKConstraintNode3D::set_disable_scale(bool p_enabled) { - disable_scale = p_enabled; -} - -bool IKConstraintNode3D::is_scale_disabled() const { - return disable_scale; -} - -void IKConstraintNode3D::set_parent(Ref p_parent) { - if (p_parent.is_valid()) { - p_parent->children.erase(this); - } - parent.set_ref(p_parent); - if (p_parent.is_valid()) { - p_parent->children.push_back(this); - } - _propagate_transform_changed(); -} - -Ref IKConstraintNode3D::get_parent() const { - return parent.get_ref(); -} - -Vector3 IKConstraintNode3D::to_local(const Vector3 &p_global) const { - return get_global_transform().affine_inverse().xform(p_global); -} - -Vector3 IKConstraintNode3D::to_global(const Vector3 &p_local) const { - return get_global_transform().xform(p_local); -} - -IKConstraintNode3D::~IKConstraintNode3D() { - cleanup(); -} - -void IKConstraintNode3D::_notification(int p_what) { - if (p_what == NOTIFICATION_PREDELETE) { - cleanup(); - } -} -void IKConstraintNode3D::cleanup() { - for (Ref &child : children) { - child->set_parent(Ref()); - } -} diff --git a/modules/constraint_ik/src/math/ik_node_3d.h b/modules/constraint_ik/src/math/ik_node_3d.h deleted file mode 100644 index b0b26e39b4ca..000000000000 --- a/modules/constraint_ik/src/math/ik_node_3d.h +++ /dev/null @@ -1,102 +0,0 @@ -/**************************************************************************/ -/* ik_node_3d.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef IK_CONSTRAINT_NODE_3D_H -#define IK_CONSTRAINT_NODE_3D_H - -#include "core/object/ref_counted.h" -#include "core/templates/list.h" - -#include "core/io/resource.h" -#include "core/math/transform_3d.h" - -class IKConstraintNode3D : public RefCounted { - GDCLASS(IKConstraintNode3D, RefCounted); - - enum TransformDirty { - DIRTY_NONE = 0, - DIRTY_VECTORS = 1, - DIRTY_LOCAL = 2, - DIRTY_GLOBAL = 4 - }; - - mutable Transform3D global_transform; - mutable Transform3D local_transform; - mutable Basis rotation; - mutable Vector3 scale = Vector3(1, 1, 1); - - mutable int dirty = DIRTY_NONE; - - WeakRef parent; - List> children; - - bool disable_scale = false; - - void _update_local_transform() const; - -protected: - void _notification(int p_what); - static void _bind_methods() { - ClassDB::bind_method(D_METHOD("_propagate_transform_changed"), &IKConstraintNode3D::_propagate_transform_changed); - ClassDB::bind_method(D_METHOD("_update_local_transform"), &IKConstraintNode3D::_update_local_transform); - ClassDB::bind_method(D_METHOD("rotate_local_with_global", "p_basis", "p_propagate"), &IKConstraintNode3D::rotate_local_with_global, DEFVAL(false)); - ClassDB::bind_method(D_METHOD("set_transform", "p_transform"), &IKConstraintNode3D::set_transform); - ClassDB::bind_method(D_METHOD("set_global_transform", "p_transform"), &IKConstraintNode3D::set_global_transform); - ClassDB::bind_method(D_METHOD("get_transform"), &IKConstraintNode3D::get_transform); - ClassDB::bind_method(D_METHOD("get_global_transform"), &IKConstraintNode3D::get_global_transform); - ClassDB::bind_method(D_METHOD("set_disable_scale", "p_enabled"), &IKConstraintNode3D::set_disable_scale); - ClassDB::bind_method(D_METHOD("is_scale_disabled"), &IKConstraintNode3D::is_scale_disabled); - ClassDB::bind_method(D_METHOD("set_parent", "p_parent"), &IKConstraintNode3D::set_parent); - ClassDB::bind_method(D_METHOD("get_parent"), &IKConstraintNode3D::get_parent); - ClassDB::bind_method(D_METHOD("to_local", "p_global"), &IKConstraintNode3D::to_local); - ClassDB::bind_method(D_METHOD("to_global", "p_local"), &IKConstraintNode3D::to_global); - } - -public: - void _propagate_transform_changed(); - void set_transform(const Transform3D &p_transform); - void set_global_transform(const Transform3D &p_transform); - Transform3D get_transform() const; - Transform3D get_global_transform() const; - - void set_disable_scale(bool p_enabled); - bool is_scale_disabled() const; - - void set_parent(Ref p_parent); - Ref get_parent() const; - - Vector3 to_local(const Vector3 &p_global) const; - Vector3 to_global(const Vector3 &p_local) const; - void rotate_local_with_global(const Basis &p_basis, bool p_propagate = false); - void cleanup(); - ~IKConstraintNode3D(); -}; - -#endif // IK_NODE_3D_H diff --git a/modules/constraint_ik/src/math/qcp.cpp b/modules/constraint_ik/src/math/qcp.cpp deleted file mode 100644 index 1e16b7151de5..000000000000 --- a/modules/constraint_ik/src/math/qcp.cpp +++ /dev/null @@ -1,269 +0,0 @@ -/**************************************************************************/ -/* qcp.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "qcp.h" - -QuaternionCharacteristicPolynomial::QuaternionCharacteristicPolynomial(double p_evec_prec) { - eigenvector_precision = p_evec_prec; -} - -void QuaternionCharacteristicPolynomial::set(PackedVector3Array &r_target, PackedVector3Array &r_moved) { - target = r_target; - moved = r_moved; - transformation_calculated = false; - inner_product_calculated = false; -} - -Quaternion QuaternionCharacteristicPolynomial::_get_rotation() { - Quaternion result; - if (!transformation_calculated) { - if (!inner_product_calculated) { - inner_product(target, moved); - } - result = calculate_rotation(); - transformation_calculated = true; - } - return result; -} - -Quaternion QuaternionCharacteristicPolynomial::calculate_rotation() { - Quaternion result; - - if (moved.size() == 1) { - Vector3 u = moved[0]; - Vector3 v = target[0]; - double norm_product = u.length() * v.length(); - - if (norm_product == 0.0) { - return Quaternion(); - } - - double dot = u.dot(v); - - if (dot < ((2.0e-15 - 1.0) * norm_product)) { - Vector3 w = u.normalized(); - result = Quaternion(w.x, w.y, w.z, 0.0f).normalized(); - } else { - double q0 = Math::sqrt(0.5 * (1.0 + dot / norm_product)); - double coeff = 1.0 / (2.0 * q0 * norm_product); - Vector3 q = v.cross(u).normalized(); - result = Quaternion(coeff * q.x, coeff * q.y, coeff * q.z, q0).normalized(); - } - } else { - double a13 = -sum_xz_minus_zx; - double a14 = sum_xy_minus_yx; - double a21 = sum_yz_minus_zy; - double a22 = sum_xx_minus_yy - sum_zz - max_eigenvalue; - double a23 = sum_xy_plus_yx; - double a24 = sum_xz_plus_zx; - double a31 = a13; - double a32 = a23; - double a33 = sum_yy - sum_xx - sum_zz - max_eigenvalue; - double a34 = sum_yz_plus_zy; - double a41 = a14; - double a42 = a24; - double a43 = a34; - double a44 = sum_zz - sum_xx_plus_yy - max_eigenvalue; - - double a3344_4334 = a33 * a44 - a43 * a34; - double a3244_4234 = a32 * a44 - a42 * a34; - double a3243_4233 = a32 * a43 - a42 * a33; - double a3143_4133 = a31 * a43 - a41 * a33; - double a3144_4134 = a31 * a44 - a41 * a34; - double a3142_4132 = a31 * a42 - a41 * a32; - - double quaternion_w = a22 * a3344_4334 - a23 * a3244_4234 + a24 * a3243_4233; - double quaternion_x = -a21 * a3344_4334 + a23 * a3144_4134 - a24 * a3143_4133; - double quaternion_y = a21 * a3244_4234 - a22 * a3144_4134 + a24 * a3142_4132; - double quaternion_z = -a21 * a3243_4233 + a22 * a3143_4133 - a23 * a3142_4132; - double qsqr = quaternion_w * quaternion_w + quaternion_x * quaternion_x + quaternion_y * quaternion_y + quaternion_z * quaternion_z; - - if (qsqr < eigenvector_precision) { - result = Quaternion(); - } else { - quaternion_x *= -1; - quaternion_y *= -1; - quaternion_z *= -1; - double min = quaternion_w; - min = quaternion_x < min ? quaternion_x : min; - min = quaternion_y < min ? quaternion_y : min; - min = quaternion_z < min ? quaternion_z : min; - quaternion_w /= min; - quaternion_x /= min; - quaternion_y /= min; - quaternion_z /= min; - result = Quaternion(quaternion_x, quaternion_y, quaternion_z, quaternion_w).normalized(); - } - } - - return result; -} - -void QuaternionCharacteristicPolynomial::translate(Vector3 r_translate, PackedVector3Array &r_x) { - for (Vector3 &p : r_x) { - p += r_translate; - } -} - -Vector3 QuaternionCharacteristicPolynomial::_get_translation() { - return target_center - moved_center; -} - -Vector3 QuaternionCharacteristicPolynomial::move_to_weighted_center(PackedVector3Array &r_to_center, Vector &r_weight) { - Vector3 center; - double total_weight = 0; - bool weight_is_empty = r_weight.is_empty(); - int size = r_to_center.size(); - - for (int i = 0; i < size; i++) { - if (!weight_is_empty) { - total_weight += r_weight[i]; - center += r_to_center[i] * r_weight[i]; - } else { - center += r_to_center[i]; - total_weight++; - } - } - - if (total_weight > 0) { - center /= total_weight; - } - - return center; -} - -void QuaternionCharacteristicPolynomial::inner_product(PackedVector3Array &coords1, PackedVector3Array &coords2) { - Vector3 weighted_coord1, weighted_coord2; - double sum_of_squares1 = 0, sum_of_squares2 = 0; - - sum_xx = 0; - sum_xy = 0; - sum_xz = 0; - sum_yx = 0; - sum_yy = 0; - sum_yz = 0; - sum_zx = 0; - sum_zy = 0; - sum_zz = 0; - - bool weight_is_empty = weight.is_empty(); - int size = coords1.size(); - - for (int i = 0; i < size; i++) { - if (!weight_is_empty) { - weighted_coord1 = weight[i] * coords1[i]; - sum_of_squares1 += weighted_coord1.dot(coords1[i]); - } else { - weighted_coord1 = coords1[i]; - sum_of_squares1 += weighted_coord1.dot(weighted_coord1); - } - - weighted_coord2 = coords2[i]; - - sum_of_squares2 += weight_is_empty ? weighted_coord2.dot(weighted_coord2) : (weight[i] * weighted_coord2.dot(weighted_coord2)); - - sum_xx += (weighted_coord1.x * weighted_coord2.x); - sum_xy += (weighted_coord1.x * weighted_coord2.y); - sum_xz += (weighted_coord1.x * weighted_coord2.z); - - sum_yx += (weighted_coord1.y * weighted_coord2.x); - sum_yy += (weighted_coord1.y * weighted_coord2.y); - sum_yz += (weighted_coord1.y * weighted_coord2.z); - - sum_zx += (weighted_coord1.z * weighted_coord2.x); - sum_zy += (weighted_coord1.z * weighted_coord2.y); - sum_zz += (weighted_coord1.z * weighted_coord2.z); - } - - double initial_eigenvalue = (sum_of_squares1 + sum_of_squares2) * 0.5; - - sum_xz_plus_zx = sum_xz + sum_zx; - sum_yz_plus_zy = sum_yz + sum_zy; - sum_xy_plus_yx = sum_xy + sum_yx; - sum_yz_minus_zy = sum_yz - sum_zy; - sum_xz_minus_zx = sum_xz - sum_zx; - sum_xy_minus_yx = sum_xy - sum_yx; - sum_xx_plus_yy = sum_xx + sum_yy; - sum_xx_minus_yy = sum_xx - sum_yy; - max_eigenvalue = initial_eigenvalue; - - inner_product_calculated = true; -} - -Quaternion QuaternionCharacteristicPolynomial::_weighted_superpose(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool translate) { - set(p_moved, p_target, p_weight, translate); - return _get_rotation(); -} - -void QuaternionCharacteristicPolynomial::set(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool p_translate) { - transformation_calculated = false; - inner_product_calculated = false; - - moved = p_moved; - target = p_target; - weight = p_weight; - - if (p_translate) { - moved_center = move_to_weighted_center(moved, weight); - w_sum = 0; // set wsum to 0 so we don't double up. - target_center = move_to_weighted_center(target, weight); - translate(moved_center * -1, moved); - translate(target_center * -1, target); - } else { - if (!p_weight.is_empty()) { - for (int i = 0; i < p_weight.size(); i++) { - w_sum += p_weight[i]; - } - } else { - w_sum = p_moved.size(); - } - } -} - -void QuaternionCharacteristicPolynomial::_bind_methods() { - ClassDB::bind_static_method("QuaternionCharacteristicPolynomial", - D_METHOD("weighted_superpose", "moved", "target", - "weight", "translate", "precision"), - &QuaternionCharacteristicPolynomial::weighted_superpose); -} - -Array QuaternionCharacteristicPolynomial::weighted_superpose(PackedVector3Array p_moved, - PackedVector3Array p_target, - Vector p_weight, bool p_translate, - double p_precision) { - QuaternionCharacteristicPolynomial qcp(p_precision); - Quaternion rotation = - qcp._weighted_superpose(p_moved, p_target, p_weight, p_translate); - Vector3 translation = qcp._get_translation(); - Array result; - result.push_back(rotation); - result.push_back(translation); - return result; -} diff --git a/modules/constraint_ik/src/math/qcp.h b/modules/constraint_ik/src/math/qcp.h deleted file mode 100644 index 274b74fb02ff..000000000000 --- a/modules/constraint_ik/src/math/qcp.h +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************/ -/* qcp.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef QCP_H -#define QCP_H - -#include "core/math/vector3.h" -#include "core/object/class_db.h" -#include "core/object/object.h" -#include "core/variant/variant.h" - -/** - * Implementation of the Quaternion-Based Characteristic Polynomial algorithm - * for RMSD and Superposition calculations. - * - * Usage: - * 1. Create a QCP object with two Vector3 arrays of equal length as input. - * The input coordinates are not changed. - * 2. Optionally, provide weighting factors [0 - 1] for each point. - * 3. For maximum efficiency, create a QCP object once and reuse it. - * - * A. Calculate rmsd only: double rmsd = qcp.getRmsd(); - * B. Calculate a 4x4 transformation (Quaternion and translation) matrix: Matrix4f trans = qcp.getTransformationMatrix(); - * C. Get transformed points (y superposed onto the reference x): Vector3[] ySuperposed = qcp.getTransformedCoordinates(); - * - * Citations: - * - Liu P, Agrafiotis DK, & Theobald DL (2011) Reply to comment on: "Fast determination of the optimal Quaternionation matrix for macromolecular superpositions." Journal of Computational Chemistry 32(1):185-186. [http://dx.doi.org/10.1002/jcc.21606] - * - Liu P, Agrafiotis DK, & Theobald DL (2010) "Fast determination of the optimal Quaternionation matrix for macromolecular superpositions." Journal of Computational Chemistry 31(7):1561-1563. [http://dx.doi.org/10.1002/jcc.21439] - * - Douglas L Theobald (2005) "Rapid calculation of RMSDs using a quaternion-based characteristic polynomial." Acta Crystallogr A 61(4):478-480. [http://dx.doi.org/10.1107/S0108767305015266] - * - * This is an adaptation of the original C code QCPQuaternion 1.4 (2012, October 10) to C++. - * The original C source code is available from http://theobald.brandeis.edu/qcp/ and was developed by: - * - Douglas L. Theobald, Department of Biochemistry, Brandeis University - * - Pu Liu, Johnson & Johnson Pharmaceutical Research and Development, L.L.C. - * - * @author Douglas L. Theobald (original C code) - * @author Pu Liu (original C code) - * @author Peter Rose (adapted to Java) - * @author Aleix Lafita (adapted to Java) - * @author Eron Gjoni (adapted to EWB IK) - * @author K. S. Ernest (iFire) Lee (adapted to ManyBoneIK) - */ - -class QuaternionCharacteristicPolynomial : Object { - GDCLASS(QuaternionCharacteristicPolynomial, Object); - double eigenvector_precision = 1E-6; - - PackedVector3Array target, moved; - Vector weight; - double w_sum = 0; - - Vector3 target_center, moved_center; - - double sum_xy = 0, sum_xz = 0, sum_yx = 0, sum_yz = 0, sum_zx = 0, sum_zy = 0; - double sum_xx_plus_yy = 0, sum_zz = 0, max_eigenvalue = 0, sum_yz_minus_zy = 0, sum_xz_minus_zx = 0, sum_xy_minus_yx = 0; - double sum_xx_minus_yy = 0, sum_xy_plus_yx = 0, sum_xz_plus_zx = 0; - double sum_yy = 0, sum_xx = 0, sum_yz_plus_zy = 0; - bool transformation_calculated = false, inner_product_calculated = false; - - void inner_product(PackedVector3Array &coords1, PackedVector3Array &coords2); - void set(PackedVector3Array &r_target, PackedVector3Array &r_moved); - Quaternion calculate_rotation(); - void set(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool p_translate); - static void translate(Vector3 r_translate, PackedVector3Array &r_x); - Vector3 move_to_weighted_center(PackedVector3Array &r_to_center, Vector &r_weight); - QuaternionCharacteristicPolynomial(double p_evec_prec); - Quaternion _weighted_superpose(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool translate); - Quaternion _get_rotation(); - Vector3 _get_translation(); - -protected: - static void _bind_methods(); - -public: - static Array weighted_superpose(PackedVector3Array p_moved, - PackedVector3Array p_target, - Vector p_weight, bool p_translate, - double p_precision = 1E-6); -}; - -#endif // QCP_H diff --git a/modules/constraint_ik/tests/test_ik_node_3d.h b/modules/constraint_ik/tests/test_ik_node_3d.h deleted file mode 100644 index 9eee9d0cacf7..000000000000 --- a/modules/constraint_ik/tests/test_ik_node_3d.h +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************/ -/* test_ik_node_3d.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef TEST_IK_NODE_3D_H -#define TEST_IK_NODE_3D_H - -#include "tests/test_macros.h" - -namespace TestIKConstraintNode3D { - -TEST_CASE("[Modules][IKConstraintNode3D] Transform operations") { - Ref node; - node.instantiate(); - - // Test set_transform and get_transform - Transform3D t; - t.origin = Vector3(1, 2, 3); - node->set_transform(t); - CHECK(node->get_transform() == t); - - // Test set_global_transform and get_global_transform - Transform3D gt; - gt.origin = Vector3(4, 5, 6); - node->set_global_transform(gt); - CHECK(node->get_global_transform() == gt); -} - -TEST_CASE("[Modules][IKConstraintNode3D] Scale operations") { - Ref node; - node.instantiate(); - - // Test set_disable_scale and is_scale_disabled - node->set_disable_scale(true); - CHECK(node->is_scale_disabled()); -} - -TEST_CASE("[Modules][IKConstraintNode3D] Parent operations") { - Ref node; - node.instantiate(); - Ref parent; - parent.instantiate(); - - // Test set_parent and get_parent - node->set_parent(parent); - CHECK(node->get_parent() == parent); -} - -TEST_CASE("[Modules][IKConstraintNode3D] Coordinate transformations") { - Ref node; - node.instantiate(); - - // Test to_local and to_global - Vector3 global(1, 2, 3); - Vector3 local = node->to_local(global); - CHECK(node->to_global(local) == global); -} - -TEST_CASE("[Modules][IKConstraintNode3D] Test local transform calculation") { - Ref node; - node.instantiate(); - - Transform3D node_transform; - node_transform.origin = Vector3(1.0, 2.0, 3.0); // Translation by (1, 2, 3) - node->set_global_transform(node_transform); - - Ref parent_node; - parent_node.instantiate(); - - Transform3D parent_transform; - parent_transform.origin = Vector3(4.0, 5.0, 6.0); // Translation by (4, 5, 6) - parent_node->set_global_transform(parent_transform); - - node->set_parent(parent_node); - - Transform3D expected_local_transform = parent_node->get_global_transform().affine_inverse() * node->get_global_transform(); - - CHECK(node->get_transform() == expected_local_transform); -} -} // namespace TestIKConstraintNode3D - -#endif // TEST_IK_NODE_3D_H diff --git a/modules/constraint_ik/tests/test_qcp.h b/modules/constraint_ik/tests/test_qcp.h deleted file mode 100644 index 5a1f21ca9fa5..000000000000 --- a/modules/constraint_ik/tests/test_qcp.h +++ /dev/null @@ -1,143 +0,0 @@ -/**************************************************************************/ -/* test_qcp.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef TEST_QCP_H -#define TEST_QCP_H - -#include "core/math/quaternion.h" -#include "modules/constraint_ik/src/math/qcp.h" -#include "tests/test_macros.h" - -namespace TestQCP { - -TEST_CASE("[Modules][QCP] No Translation") { - Quaternion expected = Quaternion(0, 0, sqrt(2) / 2, sqrt(2) / 2); - PackedVector3Array moved = { Vector3(4, 5, 6), Vector3(7, 8, 9), Vector3(1, 2, 3) }; - PackedVector3Array target = moved; - for (Vector3 &element : target) { - element = expected.xform(element); - } - Vector weight = { 1.0, 1.0, 1.0 }; // Equal weights - bool translate = false; - double epsilon = 1e-6; - Array result = QuaternionCharacteristicPolynomial::weighted_superpose(moved, target, weight, translate, epsilon); - Quaternion rotation = result[0]; - CHECK(abs(rotation.x - expected.x) < epsilon); - CHECK(abs(rotation.y - expected.y) < epsilon); - CHECK(abs(rotation.z - expected.z) < epsilon); - CHECK(abs(rotation.w - expected.w) < epsilon); - Vector3 result_translation = result[1]; - CHECK(result_translation.is_zero_approx()); -} - -TEST_CASE("[Modules][QCP] Different Weights") { - Quaternion expected = Quaternion(0, 0, sqrt(2) / 2, sqrt(2) / 2); - PackedVector3Array moved = { Vector3(4, 5, 6), Vector3(7, 8, 9), Vector3(1, 2, 3) }; - PackedVector3Array target = moved; - for (Vector3 &element : target) { - element = expected.xform(element); - } - Vector weight = { 0.5, 1.0, 1.5 }; // Different weights - bool translate = false; - double epsilon = 1e-6; - - Array result = QuaternionCharacteristicPolynomial::weighted_superpose(moved, target, weight, translate, epsilon); - Quaternion rotation = result[0]; - CHECK(abs(rotation.x - expected.x) < epsilon); - CHECK(abs(rotation.y - expected.y) < epsilon); - CHECK(abs(rotation.z - expected.z) < epsilon); - CHECK(abs(rotation.w - expected.w) < epsilon); -} - -TEST_CASE("[Modules][QCP] Zero Weights") { - Quaternion expected = Quaternion(0, 0, sqrt(2) / 2, sqrt(2) / 2); - PackedVector3Array moved = { Vector3(4, 5, 6), Vector3(7, 8, 9), Vector3(1, 2, 3) }; - PackedVector3Array target = moved; - for (Vector3 &element : target) { - element = expected.xform(element); - } - Vector weight = { 0.0, 0.0, 0.0 }; // Zero weights - bool translate = false; - double epsilon = 1e-6; - - Array result = QuaternionCharacteristicPolynomial::weighted_superpose(moved, target, weight, translate, epsilon); - Quaternion rotation = result[0]; - CHECK(abs(rotation.x - expected.x) < epsilon); - CHECK(abs(rotation.y - expected.y) < epsilon); - CHECK(abs(rotation.z - expected.z) < epsilon); - CHECK(abs(rotation.w - expected.w) < epsilon); -} - -TEST_CASE("[Modules][QCP] Identity Rotation") { - Quaternion expected = Quaternion(); - PackedVector3Array moved = { Vector3(4, 5, 6), Vector3(7, 8, 9), Vector3(1, 2, 3) }; - PackedVector3Array target = moved; - Vector weight = { 1.0, 1.0, 1.0 }; // Equal weights - bool translate = false; - double epsilon = 1e-6; - - Array result = QuaternionCharacteristicPolynomial::weighted_superpose(moved, target, weight, translate, epsilon); - Quaternion rotation = result[0]; - CHECK(abs(rotation.x - expected.x) < epsilon); - CHECK(abs(rotation.y - expected.y) < epsilon); - CHECK(abs(rotation.z - expected.z) < epsilon); - CHECK(abs(rotation.w - expected.w) < epsilon); -} - -TEST_CASE("[Modules][QCP] Random Rotation and Translation") { - Quaternion expected_rotation = Quaternion(0.1, 0.2, 0.3, 0.4).normalized(); - Vector3 expected_translation = Vector3(1, 2, 3); - PackedVector3Array moved = { Vector3(4, 5, 6), Vector3(7, 8, 9), Vector3(1, 2, 3) }; - PackedVector3Array target = moved; - for (Vector3 &element : target) { - element = expected_rotation.xform(element + expected_translation); - } - Vector weight = { 1.0, 1.0, 1.0 }; // Equal weights - bool translate = true; - double epsilon = 1e-6; - - Array result = QuaternionCharacteristicPolynomial::weighted_superpose(moved, target, weight, translate, epsilon); - Quaternion rotation = result[0]; - Vector3 translation = result[1]; - CHECK(abs(rotation.x - expected_rotation.x) < epsilon); - CHECK(abs(rotation.y - expected_rotation.y) < epsilon); - CHECK(abs(rotation.z - expected_rotation.z) < epsilon); - CHECK(abs(rotation.w - expected_rotation.w) < epsilon); - - CHECK(translate); - Vector3 translation_result = expected_rotation.xform_inv(translation); - CHECK(abs(translation_result.x - expected_translation.x) < epsilon); - CHECK(abs(translation_result.y - expected_translation.y) < epsilon); - CHECK(abs(translation_result.z - expected_translation.z) < epsilon); -} - -} // namespace TestQCP - -#endif // TEST_QCP_H diff --git a/modules/many_bone_ik/.gitrepo b/modules/many_bone_ik/.gitrepo new file mode 100644 index 000000000000..6b763294c5cd --- /dev/null +++ b/modules/many_bone_ik/.gitrepo @@ -0,0 +1,12 @@ +; DO NOT EDIT (unless you know what you are doing) +; +; This subdirectory is a git "subrepo", and this file is maintained by the +; git-subrepo command. See https://github.com/ingydotnet/git-subrepo#readme +; +[subrepo] + remote = https://github.com/V-Sekai/many_bone_ik.git + branch = main + commit = b6646697056c93d3296aca79fbf0093a307f0cb2 + parent = 251a9ddb2b40ab42487b86928ed97940af6b602c + method = merge + cmdver = 0.4.9 diff --git a/modules/many_bone_ik/SCsub b/modules/many_bone_ik/SCsub index 6ac6849a687a..715fb0101706 100644 --- a/modules/many_bone_ik/SCsub +++ b/modules/many_bone_ik/SCsub @@ -10,3 +10,6 @@ env_many_bone_ik.add_source_files(env.modules_sources, "constraints/*.cpp") env_many_bone_ik.add_source_files(env.modules_sources, "src/math/*.cpp") env_many_bone_ik.add_source_files(env.modules_sources, "src/*.cpp") env_many_bone_ik.add_source_files(env.modules_sources, "*.cpp") + +if env.editor_build: + env_many_bone_ik.add_source_files(env.modules_sources, "editor/*.cpp") diff --git a/modules/many_bone_ik/config.py b/modules/many_bone_ik/config.py index 0c100cebff4d..b27d55d5d10c 100644 --- a/modules/many_bone_ik/config.py +++ b/modules/many_bone_ik/config.py @@ -13,7 +13,10 @@ def get_doc_classes(): "IKEffector3D", "IKBoneSegment3D", "IKEffectorTemplate3D", + "IKKusudama3D", + "IKRay3D", "IKNode3D", + "IKLimitCone3D", ] diff --git a/modules/many_bone_ik/doc_classes/IKBone3D.xml b/modules/many_bone_ik/doc_classes/IKBone3D.xml index f2b8bbea0e78..d7eb00fa46f1 100644 --- a/modules/many_bone_ik/doc_classes/IKBone3D.xml +++ b/modules/many_bone_ik/doc_classes/IKBone3D.xml @@ -9,6 +9,24 @@ + + + + Returns the IKKusudama3D object representing the constraint of the bone. + + + + + + Returns the IKNode3D object representing the orientation transform of the constraint. + + + + + + Returns the IKNode3D object representing the twist transform of the constraint. + + diff --git a/modules/constraint_ik/doc_classes/IKKusudama3D.xml b/modules/many_bone_ik/doc_classes/IKKusudama3D.xml similarity index 100% rename from modules/constraint_ik/doc_classes/IKKusudama3D.xml rename to modules/many_bone_ik/doc_classes/IKKusudama3D.xml diff --git a/modules/constraint_ik/doc_classes/IKLimitCone3D.xml b/modules/many_bone_ik/doc_classes/IKLimitCone3D.xml similarity index 100% rename from modules/constraint_ik/doc_classes/IKLimitCone3D.xml rename to modules/many_bone_ik/doc_classes/IKLimitCone3D.xml diff --git a/modules/constraint_ik/doc_classes/IKRay3D.xml b/modules/many_bone_ik/doc_classes/IKRay3D.xml similarity index 100% rename from modules/constraint_ik/doc_classes/IKRay3D.xml rename to modules/many_bone_ik/doc_classes/IKRay3D.xml diff --git a/modules/many_bone_ik/doc_classes/ManyBoneIK3D.xml b/modules/many_bone_ik/doc_classes/ManyBoneIK3D.xml index 1f4712890f5f..dab81b413d29 100644 --- a/modules/many_bone_ik/doc_classes/ManyBoneIK3D.xml +++ b/modules/many_bone_ik/doc_classes/ManyBoneIK3D.xml @@ -9,6 +9,13 @@ + + + + + Returns the index of the constraint with the given name. If no such constraint exists, returns -1. + + @@ -21,6 +28,25 @@ Returns the total number of bones in the IK system. + + + + Returns the total number of constraints in the IK system. + + + + + + + Returns the name of the constraint at the specified index. + + + + + + + + @@ -33,6 +59,41 @@ + + + + + + + + + + + + Returns the center of the limit cone for the kusudama at the specified index. + + + + + + + Returns the count of limit cones for the kusudama at the specified index. + + + + + + + + Returns the radius of the limit cone for the kusudama at the specified index. + + + + + + + + @@ -67,12 +128,37 @@ Returns the weight of the pin at the specified index. + + + + + + + + + + Registers the skeleton to the IK system. This should be called after all bones and constraints have been added to the system. + + + + + + Resets all constraints in the IK system to their default state. + + + + + + + Sets the total number of constraints. + + @@ -80,6 +166,13 @@ + + + + + + + @@ -101,6 +194,46 @@ + + + + + + + + + + + + + + Sets the center of the limit cone for the kusudama at the specified index. + + + + + + + + Sets the count of limit cones for the kusudama at the specified index. + + + + + + + + + Sets the radius of the limit cone for the kusudama at the specified index. + + + + + + + + + @@ -137,8 +270,18 @@ + + + + + + + + + A boolean value indicating whether the IK system is in constraint mode or not. + The default maximum number of radians a bone is allowed to rotate per solver iteration. The lower this value, the more natural the pose results. However, this will increase the number of iterations_per_frame the solver requires to converge. @@ -148,5 +291,8 @@ The number of stabilization passes performed by the solver. This can help to improve the stability of the IK solution. + + The index of the bone currently selected in the user interface. + diff --git a/modules/constraint_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp b/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp similarity index 88% rename from modules/constraint_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp rename to modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp index 6e7478cc0b76..5481406c28e2 100644 --- a/modules/constraint_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp +++ b/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp @@ -32,6 +32,7 @@ #include "core/templates/local_vector.h" #include "editor/editor_interface.h" #include "editor/editor_node.h" +#include "editor/many_bone_ik_shader.h" #include "editor/plugins/node_3d_editor_gizmos.h" #include "editor/plugins/node_3d_editor_plugin.h" #include "scene/3d/mesh_instance_3d.h" @@ -42,28 +43,27 @@ #include "../src/ik_kusudama_3d.h" #include "../src/many_bone_ik_3d.h" #include "many_bone_ik_3d_gizmo_plugin.h" -#include "many_bone_ik_shader.h" #ifdef TOOLS_ENABLED #include "editor/editor_node.h" #include "editor/editor_undo_redo_manager.h" #endif -void ConstraintIK3DGizmoPlugin::_bind_methods() { - ClassDB::bind_method(D_METHOD("_get_gizmo_name"), &ConstraintIK3DGizmoPlugin::get_gizmo_name); +void ManyBoneIK3DGizmoPlugin::_bind_methods() { + ClassDB::bind_method(D_METHOD("_get_gizmo_name"), &ManyBoneIK3DGizmoPlugin::get_gizmo_name); } -bool ConstraintIK3DGizmoPlugin::has_gizmo(Node3D *p_spatial) { - return cast_to(p_spatial); +bool ManyBoneIK3DGizmoPlugin::has_gizmo(Node3D *p_spatial) { + return cast_to(p_spatial); } -String ConstraintIK3DGizmoPlugin::get_gizmo_name() const { - return "ConstraintIK3D"; +String ManyBoneIK3DGizmoPlugin::get_gizmo_name() const { + return "ManyBoneIK3D"; } -void ConstraintIK3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { - many_bone_ik = Object::cast_to(p_gizmo->get_node_3d()); - Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); +void ManyBoneIK3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { + many_bone_ik = Object::cast_to(p_gizmo->get_node_3d()); + Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); p_gizmo->clear(); if (!skeleton || !skeleton->get_bone_count()) { return; @@ -87,11 +87,11 @@ void ConstraintIK3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { Color current_bone_color = (current_bone_idx == selected) ? selected_bone_color : bone_color; - for (const Ref &segmented_skeleton : many_bone_ik->get_segmented_skeletons()) { + for (const Ref &segmented_skeleton : many_bone_ik->get_segmented_skeletons()) { if (segmented_skeleton.is_null()) { continue; } - Ref ik_bone = segmented_skeleton->get_ik_bone(current_bone_idx); + Ref ik_bone = segmented_skeleton->get_ik_bone(current_bone_idx); if (ik_bone.is_null() || ik_bone->get_constraint().is_null()) { continue; } @@ -114,7 +114,7 @@ void ConstraintIK3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { } } -void ConstraintIK3DGizmoPlugin::create_gizmo_mesh(BoneId current_bone_idx, Ref ik_bone, EditorNode3DGizmo *p_gizmo, Color current_bone_color, Skeleton3D *many_bone_ik_skeleton, ConstraintIK3D *p_many_bone_ik) { +void ManyBoneIK3DGizmoPlugin::create_gizmo_mesh(BoneId current_bone_idx, Ref ik_bone, EditorNode3DGizmo *p_gizmo, Color current_bone_color, Skeleton3D *many_bone_ik_skeleton, ManyBoneIK3D *p_many_bone_ik) { Ref ik_kusudama = ik_bone->get_constraint(); if (ik_kusudama.is_null()) { return; @@ -261,18 +261,18 @@ void ConstraintIK3DGizmoPlugin::create_gizmo_mesh(BoneId current_bone_idx, Ref many_bone_ik_gizmo_plugin; + Ref many_bone_ik_gizmo_plugin; many_bone_ik_gizmo_plugin.instantiate(); Node3DEditor::get_singleton()->add_gizmo_plugin(many_bone_ik_gizmo_plugin); } -int ConstraintIK3DGizmoPlugin::subgizmos_intersect_ray(const EditorNode3DGizmo *p_gizmo, Camera3D *p_camera, const Vector2 &p_point) const { - Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); +int ManyBoneIK3DGizmoPlugin::subgizmos_intersect_ray(const EditorNode3DGizmo *p_gizmo, Camera3D *p_camera, const Vector2 &p_point) const { + Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); ERR_FAIL_COND_V(!skeleton, -1); if (!edit_mode) { @@ -311,8 +311,8 @@ int ConstraintIK3DGizmoPlugin::subgizmos_intersect_ray(const EditorNode3DGizmo * return -1; } -Transform3D ConstraintIK3DGizmoPlugin::get_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id) const { - Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); +Transform3D ManyBoneIK3DGizmoPlugin::get_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id) const { + Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); ERR_FAIL_COND_V(!skeleton, Transform3D()); Transform3D constraint_relative_to_the_skeleton = many_bone_ik->get_relative_transform(many_bone_ik->get_owner()).affine_inverse() * @@ -320,8 +320,8 @@ Transform3D ConstraintIK3DGizmoPlugin::get_subgizmo_transform(const EditorNode3D return constraint_relative_to_the_skeleton * skeleton->get_bone_global_pose(p_id); } -void ConstraintIK3DGizmoPlugin::set_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id, Transform3D p_transform) { - Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); +void ManyBoneIK3DGizmoPlugin::set_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id, Transform3D p_transform) { + Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); ERR_FAIL_COND(!skeleton); // Prepare for global to local. Transform3D original_to_local; @@ -352,8 +352,8 @@ void ConstraintIK3DGizmoPlugin::set_subgizmo_transform(const EditorNode3DGizmo * many_bone_ik->update_gizmos(); } -void ConstraintIK3DGizmoPlugin::commit_subgizmos(const EditorNode3DGizmo *p_gizmo, const Vector &p_ids, const Vector &p_restore, bool p_cancel) { - Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); +void ManyBoneIK3DGizmoPlugin::commit_subgizmos(const EditorNode3DGizmo *p_gizmo, const Vector &p_ids, const Vector &p_restore, bool p_cancel) { + Skeleton3D *skeleton = Object::cast_to(p_gizmo->get_node_3d())->get_skeleton(); ERR_FAIL_COND(!skeleton); Node3DEditor *ne = Node3DEditor::get_singleton(); @@ -375,7 +375,7 @@ void ConstraintIK3DGizmoPlugin::commit_subgizmos(const EditorNode3DGizmo *p_gizm ur->commit_action(); } -void ConstraintIK3DGizmoPlugin::_draw_handles() { +void ManyBoneIK3DGizmoPlugin::_draw_handles() { if (!many_bone_ik) { return; } @@ -408,7 +408,7 @@ void ConstraintIK3DGizmoPlugin::_draw_handles() { } } -void ConstraintIK3DGizmoPlugin::_draw_gizmo() { +void ManyBoneIK3DGizmoPlugin::_draw_gizmo() { if (!many_bone_ik) { return; } @@ -430,7 +430,7 @@ void ConstraintIK3DGizmoPlugin::_draw_gizmo() { } } -void ConstraintIK3DGizmoPlugin::_update_gizmo_visible() { +void ManyBoneIK3DGizmoPlugin::_update_gizmo_visible() { if (!many_bone_ik) { return; } @@ -462,7 +462,7 @@ void ConstraintIK3DGizmoPlugin::_update_gizmo_visible() { _draw_gizmo(); } -void ConstraintIK3DGizmoPlugin::_subgizmo_selection_change() { +void ManyBoneIK3DGizmoPlugin::_subgizmo_selection_change() { if (!many_bone_ik) { return; } @@ -499,16 +499,16 @@ void ConstraintIK3DGizmoPlugin::_subgizmo_selection_change() { } } -void ConstraintIK3DGizmoPlugin::edit_mode_toggled(const bool pressed) { +void ManyBoneIK3DGizmoPlugin::edit_mode_toggled(const bool pressed) { edit_mode = pressed; _update_gizmo_visible(); } -void ConstraintIK3DGizmoPlugin::_hide_handles() { +void ManyBoneIK3DGizmoPlugin::_hide_handles() { handles_mesh_instance->hide(); } -void ConstraintIK3DGizmoPlugin::_notifications(int32_t p_what) { +void ManyBoneIK3DGizmoPlugin::_notifications(int32_t p_what) { switch (p_what) { case EditorNode3DGizmoPlugin::NOTIFICATION_POSTINITIALIZE: { kusudama_shader->set_code(MANY_BONE_IKKUSUDAMA_SHADER); @@ -553,7 +553,7 @@ void fragment() { edit_mode_button->set_toggle_mode(true); edit_mode_button->set_focus_mode(Control::FOCUS_NONE); edit_mode_button->set_tooltip_text(TTR("Edit Mode\nShow buttons on joints.")); - edit_mode_button->connect("toggled", callable_mp(this, &ConstraintIK3DGizmoPlugin::edit_mode_toggled)); + edit_mode_button->connect("toggled", callable_mp(this, &ManyBoneIK3DGizmoPlugin::edit_mode_toggled)); edit_mode = false; create_material("lines_primary", Color(0.93725490570068, 0.19215686619282, 0.22352941334248), true, true, true); diff --git a/modules/constraint_ik/editor/many_bone_ik_3d_gizmo_plugin.h b/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.h similarity index 92% rename from modules/constraint_ik/editor/many_bone_ik_3d_gizmo_plugin.h rename to modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.h index cac397d9cbc1..99c7e3fd74e3 100644 --- a/modules/constraint_ik/editor/many_bone_ik_3d_gizmo_plugin.h +++ b/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.h @@ -49,8 +49,8 @@ class PhysicalBone3D; class ManyBoneIKEditorPlugin; class Button; -class ConstraintIK3DGizmoPlugin : public EditorNode3DGizmoPlugin { - GDCLASS(ConstraintIK3DGizmoPlugin, EditorNode3DGizmoPlugin); +class ManyBoneIK3DGizmoPlugin : public EditorNode3DGizmoPlugin { + GDCLASS(ManyBoneIK3DGizmoPlugin, EditorNode3DGizmoPlugin); Ref kusudama_shader = memnew(Shader); Ref unselected_mat; @@ -61,7 +61,7 @@ class ConstraintIK3DGizmoPlugin : public EditorNode3DGizmoPlugin { Ref handles_mesh = memnew(ImmediateMesh); Ref handle_material = memnew(ShaderMaterial); Ref handle_shader; - ConstraintIK3D *many_bone_ik = nullptr; + ManyBoneIK3D *many_bone_ik = nullptr; Button *edit_mode_button = nullptr; bool edit_mode = false; @@ -76,7 +76,7 @@ class ConstraintIK3DGizmoPlugin : public EditorNode3DGizmoPlugin { String get_gizmo_name() const override; void redraw(EditorNode3DGizmo *p_gizmo) override; int32_t get_priority() const override; - void create_gizmo_mesh(BoneId current_bone_idx, Ref ik_bone, EditorNode3DGizmo *p_gizmo, Color current_bone_color, Skeleton3D *many_bone_ik_skeleton, ConstraintIK3D *p_many_bone_ik); + void create_gizmo_mesh(BoneId current_bone_idx, Ref ik_bone, EditorNode3DGizmo *p_gizmo, Color current_bone_color, Skeleton3D *many_bone_ik_skeleton, ManyBoneIK3D *p_many_bone_ik); int subgizmos_intersect_ray(const EditorNode3DGizmo *p_gizmo, Camera3D *p_camera, const Vector2 &p_point) const override; Transform3D get_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id) const override; void set_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id, Transform3D p_transform) override; diff --git a/modules/constraint_ik/editor/many_bone_ik_shader.h b/modules/many_bone_ik/editor/many_bone_ik_shader.h similarity index 100% rename from modules/constraint_ik/editor/many_bone_ik_shader.h rename to modules/many_bone_ik/editor/many_bone_ik_shader.h diff --git a/modules/many_bone_ik/register_types.cpp b/modules/many_bone_ik/register_types.cpp index 797c6e9e3790..5b7ed4fd15c0 100644 --- a/modules/many_bone_ik/register_types.cpp +++ b/modules/many_bone_ik/register_types.cpp @@ -33,16 +33,31 @@ #include "src/ik_bone_3d.h" #include "src/ik_effector_3d.h" #include "src/ik_effector_template_3d.h" +#include "src/ik_kusudama_3d.h" #include "src/many_bone_ik_3d.h" +#ifdef TOOLS_ENABLED +#include "editor/many_bone_ik_3d_gizmo_plugin.h" +#endif + void initialize_many_bone_ik_module(ModuleInitializationLevel p_level) { if (p_level == MODULE_INITIALIZATION_LEVEL_SCENE) { + } +#ifdef TOOLS_ENABLED + if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) { + EditorPlugins::add_by_type(); + } +#endif + if (p_level == MODULE_INITIALIZATION_LEVEL_SERVERS) { GDREGISTER_CLASS(IKEffectorTemplate3D); GDREGISTER_CLASS(ManyBoneIK3D); GDREGISTER_CLASS(IKBone3D); GDREGISTER_CLASS(IKNode3D); GDREGISTER_CLASS(IKEffector3D); GDREGISTER_CLASS(IKBoneSegment3D); + GDREGISTER_CLASS(IKKusudama3D); + GDREGISTER_CLASS(IKRay3D); + GDREGISTER_CLASS(IKLimitCone3D); } } diff --git a/modules/many_bone_ik/src/ik_bone_3d.cpp b/modules/many_bone_ik/src/ik_bone_3d.cpp index f1429e2c69f0..cfd400612880 100644 --- a/modules/many_bone_ik/src/ik_bone_3d.cpp +++ b/modules/many_bone_ik/src/ik_bone_3d.cpp @@ -29,6 +29,7 @@ /**************************************************************************/ #include "ik_bone_3d.h" +#include "ik_kusudama_3d.h" #include "many_bone_ik_3d.h" #include "math/ik_node_3d.h" #include @@ -100,6 +101,24 @@ void IKBone3D::update_default_constraint_transform() { Transform3D set_constraint_twist_transform = get_set_constraint_twist_transform(); constraint_twist_transform->set_global_transform(set_constraint_twist_transform); + + if (constraint.is_null()) { + return; + } + + TypedArray cones = constraint->get_open_cones(); + Vector3 direction; + if (cones.size() == 0) { + direction = bone_direction_transform->get_global_transform().basis.get_column(Vector3::AXIS_Y); + } else { + float total_radius_sum = calculate_total_radius_sum(cones); + direction = calculate_weighted_direction(cones, total_radius_sum); + direction -= constraint_orientation_transform->get_global_transform().origin; + } + + Vector3 twist_axis = set_constraint_twist_transform.basis.get_column(Vector3::AXIS_Y); + Quaternion align_dir = Quaternion(twist_axis, direction); + constraint_twist_transform->rotate_local_with_global(align_dir); } Ref IKBone3D::get_parent() const { @@ -171,6 +190,9 @@ void IKBone3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_pin"), &IKBone3D::get_pin); ClassDB::bind_method(D_METHOD("set_pin", "pin"), &IKBone3D::set_pin); ClassDB::bind_method(D_METHOD("is_pinned"), &IKBone3D::is_pinned); + ClassDB::bind_method(D_METHOD("get_constraint"), &IKBone3D::get_constraint); + ClassDB::bind_method(D_METHOD("get_constraint_orientation_transform"), &IKBone3D::get_constraint_orientation_transform); + ClassDB::bind_method(D_METHOD("get_constraint_twist_transform"), &IKBone3D::get_constraint_twist_transform); } IKBone3D::IKBone3D(StringName p_bone, Skeleton3D *p_skeleton, const Ref &p_parent, Vector> &p_pins, float p_default_dampening, @@ -203,7 +225,12 @@ IKBone3D::IKBone3D(StringName p_bone, Skeleton3D *p_skeleton, const Refget_iterations_per_frame(); - float returnfulness = 0.0; + if (get_constraint().is_null()) { + Ref new_constraint; + new_constraint.instantiate(); + add_constraint(new_constraint); + } + float returnfulness = get_constraint()->get_resistance(); float falloff = 0.2f; half_returnfulness_dampened.resize(iterations); cos_half_returnfulness_dampened.resize(iterations); @@ -225,6 +252,14 @@ void IKBone3D::set_cos_half_dampen(float p_cos_half_dampen) { cos_half_dampen = p_cos_half_dampen; } +Ref IKBone3D::get_constraint() const { + return constraint; +} + +void IKBone3D::add_constraint(Ref p_constraint) { + constraint = p_constraint; +} + Ref IKBone3D::get_ik_transform() { return godot_skeleton_aligned_transform; } @@ -249,6 +284,20 @@ Ref IKBone3D::get_bone_direction_transform() { return bone_direction_transform; } +bool IKBone3D::is_orientationally_constrained() { + if (get_constraint().is_null()) { + return false; + } + return get_constraint()->is_orientationally_constrained(); +} + +bool IKBone3D::is_axially_constrained() { + if (get_constraint().is_null()) { + return false; + } + return get_constraint()->is_axially_constrained(); +} + Vector &IKBone3D::get_cos_half_returnfullness_dampened() { return cos_half_returnfulness_dampened; } @@ -286,3 +335,30 @@ Transform3D IKBone3D::get_parent_bone_aligned_transform() { Transform3D IKBone3D::get_set_constraint_twist_transform() const { return constraint_orientation_transform->get_global_transform(); } + +float IKBone3D::calculate_total_radius_sum(const TypedArray &p_cones) const { + float total_radius_sum = 0.0f; + for (int32_t i = 0; i < p_cones.size(); ++i) { + const Ref &cone = p_cones[i]; + if (cone.is_null()) { + break; + } + total_radius_sum += cone->get_radius(); + } + return total_radius_sum; +} + +Vector3 IKBone3D::calculate_weighted_direction(const TypedArray &p_cones, float p_total_radius_sum) const { + Vector3 direction = Vector3(); + for (int32_t i = 0; i < p_cones.size(); ++i) { + const Ref &cone = p_cones[i]; + if (cone.is_null()) { + break; + } + float weight = cone->get_radius() / p_total_radius_sum; + direction += cone->get_control_point() * weight; + } + direction.normalize(); + direction = constraint_orientation_transform->get_global_transform().basis.xform(direction); + return direction; +} diff --git a/modules/many_bone_ik/src/ik_bone_3d.h b/modules/many_bone_ik/src/ik_bone_3d.h index 72075b72693e..17b6062f6771 100644 --- a/modules/many_bone_ik/src/ik_bone_3d.h +++ b/modules/many_bone_ik/src/ik_bone_3d.h @@ -32,6 +32,8 @@ #define IK_BONE_3D_H #include "ik_effector_template_3d.h" +#include "ik_kusudama_3d.h" +#include "ik_open_cone_3d.h" #include "math/ik_node_3d.h" #include "core/io/resource.h" @@ -58,6 +60,7 @@ class IKBone3D : public Resource { Vector cos_half_returnfulness_dampened; Vector half_returnfulness_dampened; double stiffness = 0.0; + Ref constraint; // In the space of the local parent bone transform. // The origin is the origin of the bone direction transform // Can be independent and should be calculated @@ -78,6 +81,8 @@ class IKBone3D : public Resource { void set_half_returnfullness_dampened(const Vector &p_value); void set_stiffness(double p_stiffness); double get_stiffness() const; + bool is_axially_constrained(); + bool is_orientationally_constrained(); Transform3D get_bone_direction_global_pose() const; Ref get_bone_direction_transform(); void set_bone_direction_transform(Ref p_bone_direction); @@ -86,6 +91,8 @@ class IKBone3D : public Resource { Ref get_constraint_orientation_transform(); Ref get_constraint_twist_transform(); void update_default_constraint_transform(); + void add_constraint(Ref p_constraint); + Ref get_constraint() const; void set_bone_id(BoneId p_bone_id, Skeleton3D *p_skeleton = nullptr); BoneId get_bone_id() const; void set_parent(const Ref &p_parent); @@ -108,6 +115,8 @@ class IKBone3D : public Resource { void set_cos_half_dampen(float p_cos_half_dampen); Transform3D get_parent_bone_aligned_transform(); Transform3D get_set_constraint_twist_transform() const; + float calculate_total_radius_sum(const TypedArray &p_cones) const; + Vector3 calculate_weighted_direction(const TypedArray &p_cones, float p_total_radius_sum) const; }; #endif // IK_BONE_3D_H diff --git a/modules/many_bone_ik/src/ik_bone_segment_3d.cpp b/modules/many_bone_ik/src/ik_bone_segment_3d.cpp index 3f19faef11f5..b0aba235f832 100644 --- a/modules/many_bone_ik/src/ik_bone_segment_3d.cpp +++ b/modules/many_bone_ik/src/ik_bone_segment_3d.cpp @@ -32,6 +32,7 @@ #include "core/string/string_builder.h" #include "ik_effector_3d.h" +#include "ik_kusudama_3d.h" #include "many_bone_ik_3d.h" #include "scene/3d/skeleton_3d.h" @@ -152,6 +153,13 @@ void IKBoneSegment3D::_set_optimal_rotation(Ref p_for_bone, PackedVect Transform3D result = Transform3D(p_for_bone->get_global_pose().basis, p_for_bone->get_global_pose().origin + translation); p_for_bone->set_global_pose(result); } + bool is_parent_valid = p_for_bone->get_parent().is_valid(); + if (is_parent_valid && p_for_bone->is_orientationally_constrained()) { + p_for_bone->get_constraint()->snap_to_orientation_limit(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_orientation_transform(), bone_damp, p_for_bone->get_cos_half_dampen()); + } + if (is_parent_valid && p_for_bone->is_axially_constrained()) { + p_for_bone->get_constraint()->set_snap_to_twist_limit(p_for_bone->get_bone_direction_transform(), p_for_bone->get_ik_transform(), p_for_bone->get_constraint_twist_transform(), bone_damp, p_for_bone->get_cos_half_dampen()); + } if (default_stabilizing_pass_count > 0) { _update_tip_headings(p_for_bone, &tip_headings_uniform); double current_msd = _get_manual_msd(tip_headings_uniform, target_headings, heading_weights); diff --git a/modules/many_bone_ik/src/ik_bone_segment_3d.h b/modules/many_bone_ik/src/ik_bone_segment_3d.h index ba66a79324b4..e5d33c54ce2c 100644 --- a/modules/many_bone_ik/src/ik_bone_segment_3d.h +++ b/modules/many_bone_ik/src/ik_bone_segment_3d.h @@ -42,6 +42,7 @@ class IKEffector3D; class IKBone3D; +class IKLimitCone3D; class IKBoneSegment3D : public Resource { GDCLASS(IKBoneSegment3D, Resource); diff --git a/modules/many_bone_ik/src/ik_effector_3d.h b/modules/many_bone_ik/src/ik_effector_3d.h index c9b1312aa002..c0c56e359244 100644 --- a/modules/many_bone_ik/src/ik_effector_3d.h +++ b/modules/many_bone_ik/src/ik_effector_3d.h @@ -31,6 +31,8 @@ #ifndef IK_EFFECTOR_3D_H #define IK_EFFECTOR_3D_H +#include "math/ik_node_3d.h" + #include "core/object/ref_counted.h" #include "scene/3d/skeleton_3d.h" @@ -72,6 +74,7 @@ class IKEffector3D : public Resource { void set_direction_priorities(Vector3 p_direction_priorities); Vector3 get_direction_priorities() const; void update_target_global_transform(Skeleton3D *p_skeleton, ManyBoneIK3D *p_modification = nullptr); + const float MAX_KUSUDAMA_OPEN_CONES = 30; float get_motion_propagation_factor() const; void set_motion_propagation_factor(float p_motion_propagation_factor); void set_target_node(Skeleton3D *p_skeleton, const NodePath &p_target_node_path); diff --git a/modules/constraint_ik/src/ik_kusudama_3d.cpp b/modules/many_bone_ik/src/ik_kusudama_3d.cpp similarity index 95% rename from modules/constraint_ik/src/ik_kusudama_3d.cpp rename to modules/many_bone_ik/src/ik_kusudama_3d.cpp index 9ea498a3b9ca..3542a5a60586 100644 --- a/modules/constraint_ik/src/ik_kusudama_3d.cpp +++ b/modules/many_bone_ik/src/ik_kusudama_3d.cpp @@ -34,7 +34,7 @@ #include "ik_open_cone_3d.h" #include "math/ik_node_3d.h" -void IKKusudama3D::_update_constraint(Ref p_limiting_axes) { +void IKKusudama3D::_update_constraint(Ref p_limiting_axes) { // Avoiding antipodal singularities by reorienting the axes. Vector directions; @@ -114,7 +114,7 @@ void IKKusudama3D::set_axial_limits(real_t min_angle, real_t in_range) { twist_max_rot = Quaternion(z_axis, twist_max_vec); } -void IKKusudama3D::set_snap_to_twist_limit(Ref p_bone_direction, Ref p_to_set, Ref p_constraint_axes, real_t p_dampening, real_t p_cos_half_dampen) { +void IKKusudama3D::set_snap_to_twist_limit(Ref p_bone_direction, Ref p_to_set, Ref p_constraint_axes, real_t p_dampening, real_t p_cos_half_dampen) { if (!is_axially_constrained()) { return; } @@ -125,7 +125,7 @@ void IKKusudama3D::set_snap_to_twist_limit(Ref p_bone_direct Basis align_rot = (global_twist_center.inverse() * global_transform_to_set.basis).orthonormalized(); Quaternion twist_rotation, swing_rotation; // Hold the ik transform's decomposed swing and twist away from global_twist_centers's global basis. get_swing_twist(align_rot.get_rotation_quaternion(), Vector3(0, 1, 0), swing_rotation, twist_rotation); - twist_rotation = IKConstraintBoneSegment3D::clamp_to_cos_half_angle(twist_rotation, twist_half_range_half_cos); + twist_rotation = IKBoneSegment3D::clamp_to_cos_half_angle(twist_rotation, twist_half_range_half_cos); Basis recomposition = (global_twist_center * (swing_rotation * twist_rotation)).orthonormalized(); Basis rotation = parent_global_inverse * recomposition; p_to_set->set_transform(Transform3D(rotation, p_to_set->get_transform().origin)); @@ -232,7 +232,7 @@ TypedArray IKKusudama3D::get_open_cones() const { return cones; } -Vector3 IKKusudama3D::local_point_on_path_sequence(Vector3 p_in_point, Ref p_limiting_axes) { +Vector3 IKKusudama3D::local_point_on_path_sequence(Vector3 p_in_point, Ref p_limiting_axes) { double closest_point_dot = 0; Vector3 point = p_limiting_axes->get_transform().xform(p_in_point); point.normalize(); @@ -344,7 +344,7 @@ void IKKusudama3D::set_open_cones(TypedArray p_cones) { } } -void IKKusudama3D::snap_to_orientation_limit(Ref bone_direction, Ref to_set, Ref limiting_axes, real_t p_dampening, real_t p_cos_half_angle_dampen) { +void IKKusudama3D::snap_to_orientation_limit(Ref bone_direction, Ref to_set, Ref limiting_axes, real_t p_dampening, real_t p_cos_half_angle_dampen) { if (bone_direction.is_null()) { return; } diff --git a/modules/constraint_ik/src/ik_kusudama_3d.h b/modules/many_bone_ik/src/ik_kusudama_3d.h similarity index 92% rename from modules/constraint_ik/src/ik_kusudama_3d.h rename to modules/many_bone_ik/src/ik_kusudama_3d.h index 1cc42fec5964..0ef9a926d85a 100644 --- a/modules/constraint_ik/src/ik_kusudama_3d.h +++ b/modules/many_bone_ik/src/ik_kusudama_3d.h @@ -28,8 +28,8 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /**************************************************************************/ -#ifndef IK_CONSTRAINT_KUSUDAMA_3D_H -#define IK_CONSTRAINT_KUSUDAMA_3D_H +#ifndef IK_KUSUDAMA_3D_H +#define IK_KUSUDAMA_3D_H #include "ik_bone_3d.h" #include "ik_bone_segment_3d.h" @@ -43,9 +43,8 @@ #include "core/variant/typed_array.h" #include "scene/3d/node_3d.h" -class IKConstraintBone3D; +class IKBone3D; class IKLimitCone3D; -class IKConstraintNode3D; class IKKusudama3D : public Resource { GDCLASS(IKKusudama3D, Resource); @@ -89,7 +88,7 @@ class IKKusudama3D : public Resource { IKKusudama3D() {} - void _update_constraint(Ref p_limiting_axes); + void _update_constraint(Ref p_limiting_axes); void update_tangent_radii(); @@ -123,7 +122,7 @@ class IKKusudama3D : public Resource { * * @param to_set */ - void snap_to_orientation_limit(Ref p_bone_direction, Ref p_to_set, Ref p_limiting_axes, real_t p_dampening, real_t p_cos_half_angle_dampen); + void snap_to_orientation_limit(Ref p_bone_direction, Ref p_to_set, Ref p_limiting_axes, real_t p_dampening, real_t p_cos_half_angle_dampen); bool is_nan_vector(const Vector3 &vec); @@ -146,7 +145,7 @@ class IKKusudama3D : public Resource { * @param limiting_axes * @return radians of the twist required to snap bone into twist limits (0 if bone is already in twist limits) */ - void set_snap_to_twist_limit(Ref p_bone_direction, Ref p_to_set, Ref p_limiting_axes, real_t p_dampening, real_t p_cos_half_dampen); + void set_snap_to_twist_limit(Ref p_bone_direction, Ref p_to_set, Ref p_limiting_axes, real_t p_dampening, real_t p_cos_half_dampen); /** * Given a point (in local coordinates), checks to see if a ray can be extended from the Kusudama's @@ -165,7 +164,7 @@ class IKKusudama3D : public Resource { */ Vector3 get_local_point_in_limits(Vector3 in_point, Vector *in_bounds); - Vector3 local_point_on_path_sequence(Vector3 in_point, Ref limiting_axes); + Vector3 local_point_on_path_sequence(Vector3 in_point, Ref limiting_axes); /** * Add a IKLimitCone to the Kusudama. diff --git a/modules/constraint_ik/src/ik_open_cone_3d.cpp b/modules/many_bone_ik/src/ik_open_cone_3d.cpp similarity index 100% rename from modules/constraint_ik/src/ik_open_cone_3d.cpp rename to modules/many_bone_ik/src/ik_open_cone_3d.cpp diff --git a/modules/constraint_ik/src/ik_open_cone_3d.h b/modules/many_bone_ik/src/ik_open_cone_3d.h similarity index 100% rename from modules/constraint_ik/src/ik_open_cone_3d.h rename to modules/many_bone_ik/src/ik_open_cone_3d.h diff --git a/modules/constraint_ik/src/ik_ray_3d.cpp b/modules/many_bone_ik/src/ik_ray_3d.cpp similarity index 100% rename from modules/constraint_ik/src/ik_ray_3d.cpp rename to modules/many_bone_ik/src/ik_ray_3d.cpp diff --git a/modules/constraint_ik/src/ik_ray_3d.h b/modules/many_bone_ik/src/ik_ray_3d.h similarity index 100% rename from modules/constraint_ik/src/ik_ray_3d.h rename to modules/many_bone_ik/src/ik_ray_3d.h diff --git a/modules/many_bone_ik/src/many_bone_ik_3d.cpp b/modules/many_bone_ik/src/many_bone_ik_3d.cpp index 967fdf5be364..6313d4c368bf 100644 --- a/modules/many_bone_ik/src/many_bone_ik_3d.cpp +++ b/modules/many_bone_ik/src/many_bone_ik_3d.cpp @@ -35,6 +35,8 @@ #include "core/object/object.h" #include "core/string/string_name.h" #include "ik_bone_3d.h" +#include "ik_kusudama_3d.h" +#include "ik_open_cone_3d.h" #include "scene/3d/skeleton_3d.h" #include "scene/main/node.h" #include "scene/main/scene_tree.h" @@ -107,6 +109,7 @@ void ManyBoneIK3D::_update_skeleton_bones_transform() { } bone->set_skeleton_bone_pose(get_skeleton()); } + update_gizmos(); } void ManyBoneIK3D::_get_property_list(List *p_list) const { @@ -156,11 +159,64 @@ void ManyBoneIK3D::_get_property_list(List *p_list) const { p_list->push_back( PropertyInfo(Variant::VECTOR3, "pins/" + itos(pin_i) + "/direction_priorities", PROPERTY_HINT_RANGE, "0,1,0.1,or_greater", pin_usage)); } + uint32_t constraint_usage = PROPERTY_USAGE_DEFAULT; + p_list->push_back( + PropertyInfo(Variant::INT, "constraint_count", + PROPERTY_HINT_RANGE, "0,256,or_greater", constraint_usage | PROPERTY_USAGE_ARRAY | PROPERTY_USAGE_READ_ONLY, + "Kusudama Constraints,constraints/")); + RBSet existing_constraints; + for (int constraint_i = 0; constraint_i < get_constraint_count(); constraint_i++) { + PropertyInfo bone_name; + bone_name.type = Variant::STRING_NAME; + bone_name.usage = constraint_usage; + bone_name.name = "constraints/" + itos(constraint_i) + "/bone_name"; + if (get_skeleton()) { + String names; + for (int bone_i = 0; bone_i < get_skeleton()->get_bone_count(); bone_i++) { + String name = get_skeleton()->get_bone_name(bone_i); + if (existing_constraints.has(name)) { + continue; + } + name += ","; + names += name; + existing_constraints.insert(name); + } + bone_name.hint = PROPERTY_HINT_ENUM_SUGGESTION; + bone_name.hint_string = names; + } else { + bone_name.hint = PROPERTY_HINT_NONE; + bone_name.hint_string = ""; + } + p_list->push_back(bone_name); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/twist_start", PROPERTY_HINT_RANGE, "-359.9,359.9,0.1,radians,exp", constraint_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/twist_end", PROPERTY_HINT_RANGE, "-359.9,359.9,0.1,radians,exp", constraint_usage)); + p_list->push_back( + PropertyInfo(Variant::INT, "constraints/" + itos(constraint_i) + "/kusudama_open_cone_count", PROPERTY_HINT_RANGE, "0,10,1", constraint_usage | PROPERTY_USAGE_ARRAY | PROPERTY_USAGE_READ_ONLY, + "Limit Cones,constraints/" + itos(constraint_i) + "/kusudama_open_cone/")); + for (int cone_i = 0; cone_i < get_kusudama_open_cone_count(constraint_i); cone_i++) { + p_list->push_back( + PropertyInfo(Variant::VECTOR3, "constraints/" + itos(constraint_i) + "/kusudama_open_cone/" + itos(cone_i) + "/center", PROPERTY_HINT_RANGE, "-1,1,0.1,exp", constraint_usage)); + + p_list->push_back( + PropertyInfo(Variant::FLOAT, "constraints/" + itos(constraint_i) + "/kusudama_open_cone/" + itos(cone_i) + "/radius", PROPERTY_HINT_RANGE, "0,180,0.1,radians,exp", constraint_usage)); + } + p_list->push_back( + PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/kusudama_twist", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); + p_list->push_back( + PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/kusudama_orientation", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); + p_list->push_back( + PropertyInfo(Variant::TRANSFORM3D, "constraints/" + itos(constraint_i) + "/bone_direction", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_STORAGE)); + } } bool ManyBoneIK3D::_get(const StringName &p_name, Variant &r_ret) const { String name = p_name; - if (name == "pin_count") { + if (name == "constraint_count") { + r_ret = get_constraint_count(); + return true; + } else if (name == "pin_count") { r_ret = get_pin_count(); return true; } else if (name == "bone_count") { @@ -191,20 +247,62 @@ bool ManyBoneIK3D::_get(const StringName &p_name, Variant &r_ret) const { r_ret = get_pin_direction_priorities(index); return true; } + } else if (name.begins_with("constraints/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + ERR_FAIL_INDEX_V(index, constraint_count, false); + String begins = "constraints/" + itos(index) + "/kusudama_open_cone"; + if (what == "bone_name") { + ERR_FAIL_INDEX_V(index, constraint_names.size(), false); + r_ret = constraint_names[index]; + return true; + } else if (what == "twist_start") { + r_ret = get_joint_twist(index).x; + return true; + } else if (what == "twist_end") { + r_ret = get_joint_twist(index).y; + return true; + } else if (what == "kusudama_open_cone_count") { + r_ret = get_kusudama_open_cone_count(index); + return true; + } else if (name.begins_with(begins)) { + int32_t cone_index = name.get_slicec('/', 3).to_int(); + String cone_what = name.get_slicec('/', 4); + if (cone_what == "center") { + Vector3 center = get_kusudama_open_cone_center(index, cone_index); + r_ret = center; + return true; + } else if (cone_what == "radius") { + r_ret = get_kusudama_open_cone_radius(index, cone_index); + return true; + } + } else if (what == "bone_direction") { + r_ret = get_direction_transform_of_bone(index); + return true; + } else if (what == "kusudama_orientation") { + r_ret = get_orientation_transform_of_constraint(index); + return true; + } else if (what == "kusudama_twist") { + r_ret = get_twist_transform_of_constraint(index); + return true; + } } return false; } bool ManyBoneIK3D::_set(const StringName &p_name, const Variant &p_value) { String name = p_name; - if (name == "pin_count") { + if (name == "constraint_count") { + _set_constraint_count(p_value); + return true; + } else if (name == "pin_count") { set_pin_count(p_value); return true; } else if (name.begins_with("pins/")) { int index = name.get_slicec('/', 1).to_int(); String what = name.get_slicec('/', 2); if (index >= pins.size()) { - set_pin_count(pin_count); + set_pin_count(constraint_count); } if (what == "bone_name") { set_pin_bone_name(index, p_value); @@ -227,13 +325,73 @@ bool ManyBoneIK3D::_set(const StringName &p_name, const Variant &p_value) { set_pin_direction_priorities(index, p_value); return true; } + } else if (name.begins_with("constraints/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + String begins = "constraints/" + itos(index) + "/kusudama_open_cone/"; + if (index >= constraint_names.size()) { + _set_constraint_count(constraint_count); + } + if (what == "bone_name") { + set_constraint_name_at_index(index, p_value); + return true; + } else if (what == "twist_from") { + Vector2 twist_from = get_joint_twist(index); + set_joint_twist(index, Vector2(p_value, twist_from.y)); + return true; + } else if (what == "twist_range") { + Vector2 twist_range = get_joint_twist(index); + set_joint_twist(index, Vector2(twist_range.x, p_value)); + return true; + } else if (what == "kusudama_open_cone_count") { + set_kusudama_open_cone_count(index, p_value); + return true; + } else if (name.begins_with(begins)) { + int cone_index = name.get_slicec('/', 3).to_int(); + String cone_what = name.get_slicec('/', 4); + if (cone_what == "center") { + set_kusudama_open_cone_center(index, cone_index, p_value); + return true; + } else if (cone_what == "radius") { + set_kusudama_open_cone_radius(index, cone_index, p_value); + return true; + } + } else if (what == "bone_direction") { + set_direction_transform_of_bone(index, p_value); + return true; + } else if (what == "kusudama_orientation") { + set_orientation_transform_of_constraint(index, p_value); + return true; + } else if (what == "kusudama_twist") { + set_twist_transform_of_constraint(index, p_value); + return true; + } } + return false; } void ManyBoneIK3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_constraint_name_at_index", "index", "name"), &ManyBoneIK3D::set_constraint_name_at_index); ClassDB::bind_method(D_METHOD("set_total_effector_count", "count"), &ManyBoneIK3D::set_pin_count); + ClassDB::bind_method(D_METHOD("get_twist_transform_of_constraint", "index"), &ManyBoneIK3D::get_twist_transform_of_constraint); + ClassDB::bind_method(D_METHOD("set_twist_transform_of_constraint", "index", "transform"), &ManyBoneIK3D::set_twist_transform_of_constraint); + ClassDB::bind_method(D_METHOD("get_orientation_transform_of_constraint", "index"), &ManyBoneIK3D::get_orientation_transform_of_constraint); + ClassDB::bind_method(D_METHOD("set_orientation_transform_of_constraint", "index", "transform"), &ManyBoneIK3D::set_orientation_transform_of_constraint); + ClassDB::bind_method(D_METHOD("get_direction_transform_of_bone", "index"), &ManyBoneIK3D::get_direction_transform_of_bone); + ClassDB::bind_method(D_METHOD("set_direction_transform_of_bone", "index", "transform"), &ManyBoneIK3D::set_direction_transform_of_bone); + ClassDB::bind_method(D_METHOD("remove_constraint_at_index", "index"), &ManyBoneIK3D::remove_pin_at_index); + ClassDB::bind_method(D_METHOD("register_skeleton"), &ManyBoneIK3D::register_skeleton); + ClassDB::bind_method(D_METHOD("reset_constraints"), &ManyBoneIK3D::reset_constraints); ClassDB::bind_method(D_METHOD("set_dirty"), &ManyBoneIK3D::set_dirty); + ClassDB::bind_method(D_METHOD("set_kusudama_open_cone_radius", "index", "cone_index", "radius"), &ManyBoneIK3D::set_kusudama_open_cone_radius); + ClassDB::bind_method(D_METHOD("get_kusudama_open_cone_radius", "index", "cone_index"), &ManyBoneIK3D::get_kusudama_open_cone_radius); + ClassDB::bind_method(D_METHOD("set_kusudama_open_cone_center", "index", "cone_index", "center"), &ManyBoneIK3D::set_kusudama_open_cone_center); + ClassDB::bind_method(D_METHOD("get_kusudama_open_cone_center", "index", "cone_index"), &ManyBoneIK3D::get_kusudama_open_cone_center); + ClassDB::bind_method(D_METHOD("set_kusudama_open_cone_count", "index", "count"), &ManyBoneIK3D::set_kusudama_open_cone_count); + ClassDB::bind_method(D_METHOD("get_kusudama_open_cone_count", "index"), &ManyBoneIK3D::get_kusudama_open_cone_count); + ClassDB::bind_method(D_METHOD("set_joint_twist", "index", "limit"), &ManyBoneIK3D::set_joint_twist); + ClassDB::bind_method(D_METHOD("get_joint_twist", "index"), &ManyBoneIK3D::get_joint_twist); ClassDB::bind_method(D_METHOD("set_pin_motion_propagation_factor", "index", "falloff"), &ManyBoneIK3D::set_pin_motion_propagation_factor); ClassDB::bind_method(D_METHOD("get_pin_motion_propagation_factor", "index"), &ManyBoneIK3D::get_pin_motion_propagation_factor); ClassDB::bind_method(D_METHOD("get_pin_count"), &ManyBoneIK3D::get_pin_count); @@ -247,18 +405,28 @@ void ManyBoneIK3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_pin_weight", "index", "weight"), &ManyBoneIK3D::set_pin_weight); ClassDB::bind_method(D_METHOD("get_pin_weight", "index"), &ManyBoneIK3D::get_pin_weight); ClassDB::bind_method(D_METHOD("get_pin_enabled", "index"), &ManyBoneIK3D::get_pin_enabled); + ClassDB::bind_method(D_METHOD("get_constraint_name", "index"), &ManyBoneIK3D::get_constraint_name); ClassDB::bind_method(D_METHOD("get_iterations_per_frame"), &ManyBoneIK3D::get_iterations_per_frame); ClassDB::bind_method(D_METHOD("set_iterations_per_frame", "count"), &ManyBoneIK3D::set_iterations_per_frame); + ClassDB::bind_method(D_METHOD("find_constraint", "name"), &ManyBoneIK3D::find_constraint); ClassDB::bind_method(D_METHOD("find_pin", "name"), &ManyBoneIK3D::find_pin); + ClassDB::bind_method(D_METHOD("get_constraint_count"), &ManyBoneIK3D::get_constraint_count); + ClassDB::bind_method(D_METHOD("set_constraint_count", "count"), &ManyBoneIK3D::_set_constraint_count); ClassDB::bind_method(D_METHOD("get_default_damp"), &ManyBoneIK3D::get_default_damp); ClassDB::bind_method(D_METHOD("set_default_damp", "damp"), &ManyBoneIK3D::set_default_damp); ClassDB::bind_method(D_METHOD("get_bone_count"), &ManyBoneIK3D::get_bone_count); + ClassDB::bind_method(D_METHOD("set_constraint_mode", "enabled"), &ManyBoneIK3D::set_constraint_mode); + ClassDB::bind_method(D_METHOD("get_constraint_mode"), &ManyBoneIK3D::get_constraint_mode); + ClassDB::bind_method(D_METHOD("set_ui_selected_bone", "bone"), &ManyBoneIK3D::set_ui_selected_bone); + ClassDB::bind_method(D_METHOD("get_ui_selected_bone"), &ManyBoneIK3D::get_ui_selected_bone); ClassDB::bind_method(D_METHOD("set_stabilization_passes", "passes"), &ManyBoneIK3D::set_stabilization_passes); ClassDB::bind_method(D_METHOD("get_stabilization_passes"), &ManyBoneIK3D::get_stabilization_passes); ClassDB::bind_method(D_METHOD("set_effector_bone_name", "index", "name"), &ManyBoneIK3D::set_pin_bone_name); ADD_PROPERTY(PropertyInfo(Variant::INT, "iterations_per_frame", PROPERTY_HINT_RANGE, "1,150,1,or_greater"), "set_iterations_per_frame", "get_iterations_per_frame"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "default_damp", PROPERTY_HINT_RANGE, "0.01,180.0,0.1,radians,exp", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_UPDATE_ALL_IF_MODIFIED), "set_default_damp", "get_default_damp"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "constraint_mode"), "set_constraint_mode", "get_constraint_mode"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "ui_selected_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_ui_selected_bone", "get_ui_selected_bone"); ADD_PROPERTY(PropertyInfo(Variant::INT, "stabilization_passes"), "set_stabilization_passes", "get_stabilization_passes"); } @@ -282,15 +450,103 @@ void ManyBoneIK3D::set_pin_motion_propagation_factor(int32_t p_effector_index, c set_dirty(); } +void ManyBoneIK3D::_set_constraint_count(int32_t p_count) { + int32_t old_count = constraint_names.size(); + constraint_count = p_count; + constraint_names.resize(p_count); + joint_twist.resize(p_count); + kusudama_open_cone_count.resize(p_count); + kusudama_open_cones.resize(p_count); + for (int32_t constraint_i = p_count; constraint_i-- > old_count;) { + constraint_names.write[constraint_i] = String(); + kusudama_open_cone_count.write[constraint_i] = 0; + kusudama_open_cones.write[constraint_i].resize(1); + kusudama_open_cones.write[constraint_i].write[0] = Vector4(0, 1, 0, 0.01745f); + joint_twist.write[constraint_i] = Vector2(0, 0.01745f); + } + set_dirty(); + notify_property_list_changed(); +} + +int32_t ManyBoneIK3D::get_constraint_count() const { + return constraint_count; +} + +inline StringName ManyBoneIK3D::get_constraint_name(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, constraint_names.size(), StringName()); + return constraint_names[p_index]; +} + +Vector2 ManyBoneIK3D::get_joint_twist(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, joint_twist.size(), Vector2()); + return joint_twist[p_index]; +} + +void ManyBoneIK3D::set_joint_twist(int32_t p_index, Vector2 p_to) { + ERR_FAIL_INDEX(p_index, constraint_count); + joint_twist.write[p_index] = p_to; + set_dirty(); +} + int32_t ManyBoneIK3D::find_pin_id(StringName p_bone_name) { - for (int32_t pin_i = 0; pin_i < pin_count; pin_i++) { - if (constraint_names[pin_i] == p_bone_name) { - return pin_i; + for (int32_t constraint_i = 0; constraint_i < constraint_count; constraint_i++) { + if (constraint_names[constraint_i] == p_bone_name) { + return constraint_i; } } return -1; } +void ManyBoneIK3D::set_kusudama_open_cone(int32_t p_constraint_index, int32_t p_index, + Vector3 p_center, float p_radius) { + ERR_FAIL_INDEX(p_constraint_index, kusudama_open_cones.size()); + Vector cones = kusudama_open_cones.write[p_constraint_index]; + if (Math::is_zero_approx(p_center.length_squared())) { + p_center = Vector3(0.0f, 1.0f, 0.0f); + } + Vector3 center = p_center.normalized(); + Vector4 cone; + cone.x = center.x; + cone.y = center.y; + cone.z = center.z; + cone.w = p_radius; + cones.write[p_index] = cone; + kusudama_open_cones.write[p_constraint_index] = cones; + set_dirty(); +} + +float ManyBoneIK3D::get_kusudama_open_cone_radius(int32_t p_constraint_index, int32_t p_index) const { + ERR_FAIL_INDEX_V(p_constraint_index, kusudama_open_cones.size(), Math_TAU); + ERR_FAIL_INDEX_V(p_index, kusudama_open_cones[p_constraint_index].size(), Math_TAU); + return kusudama_open_cones[p_constraint_index][p_index].w; +} + +int32_t ManyBoneIK3D::get_kusudama_open_cone_count(int32_t p_constraint_index) const { + ERR_FAIL_INDEX_V(p_constraint_index, kusudama_open_cone_count.size(), 0); + return kusudama_open_cone_count[p_constraint_index]; +} + +void ManyBoneIK3D::set_kusudama_open_cone_count(int32_t p_constraint_index, int32_t p_count) { + ERR_FAIL_INDEX(p_constraint_index, kusudama_open_cone_count.size()); + ERR_FAIL_INDEX(p_constraint_index, kusudama_open_cones.size()); + int32_t old_cone_count = kusudama_open_cones[p_constraint_index].size(); + kusudama_open_cone_count.write[p_constraint_index] = p_count; + Vector &cones = kusudama_open_cones.write[p_constraint_index]; + cones.resize(p_count); + String bone_name = get_constraint_name(p_constraint_index); + Transform3D bone_transform = get_direction_transform_of_bone(p_constraint_index); + Vector3 forward_axis = -bone_transform.basis.get_column(Vector3::AXIS_Y).normalized(); + for (int32_t cone_i = p_count; cone_i-- > old_cone_count;) { + Vector4 &cone = cones.write[cone_i]; + cone.x = forward_axis.x; + cone.y = forward_axis.y; + cone.z = forward_axis.z; + cone.w = Math::deg_to_rad(0.0f); + } + set_dirty(); + notify_property_list_changed(); +} + real_t ManyBoneIK3D::get_default_damp() const { return default_damp; } @@ -306,6 +562,55 @@ StringName ManyBoneIK3D::get_pin_bone_name(int32_t p_effector_index) const { return effector_template->get_name(); } +void ManyBoneIK3D::set_kusudama_open_cone_radius(int32_t p_effector_index, int32_t p_index, float p_radius) { + ERR_FAIL_INDEX(p_effector_index, kusudama_open_cone_count.size()); + ERR_FAIL_INDEX(p_effector_index, kusudama_open_cones.size()); + ERR_FAIL_INDEX(p_index, kusudama_open_cone_count[p_effector_index]); + ERR_FAIL_INDEX(p_index, kusudama_open_cones[p_effector_index].size()); + Vector4 &cone = kusudama_open_cones.write[p_effector_index].write[p_index]; + cone.w = p_radius; + set_dirty(); +} + +void ManyBoneIK3D::set_kusudama_open_cone_center(int32_t p_effector_index, int32_t p_index, Vector3 p_center) { + ERR_FAIL_INDEX(p_effector_index, kusudama_open_cones.size()); + ERR_FAIL_INDEX(p_index, kusudama_open_cones[p_effector_index].size()); + Vector4 &cone = kusudama_open_cones.write[p_effector_index].write[p_index]; + if (Math::is_zero_approx(p_center.length_squared())) { + cone.x = 0; + cone.y = 1; + cone.z = 0; + } else { + cone.x = p_center.x; + cone.y = p_center.y; + cone.z = p_center.z; + } + set_dirty(); +} + +Vector3 ManyBoneIK3D::get_kusudama_open_cone_center(int32_t p_constraint_index, int32_t p_index) const { + if (unlikely((p_constraint_index) < 0 || (p_constraint_index) >= (kusudama_open_cones.size()))) { + ERR_PRINT_ONCE("Can't get limit cone center."); + return Vector3(0.0, 0.0, 1.0); + } + if (unlikely((p_index) < 0 || (p_index) >= (kusudama_open_cones[p_constraint_index].size()))) { + ERR_PRINT_ONCE("Can't get limit cone center."); + return Vector3(0.0, 0.0, 1.0); + } + const Vector4 &cone = kusudama_open_cones[p_constraint_index][p_index]; + Vector3 ret; + ret.x = cone.x; + ret.y = cone.y; + ret.z = cone.z; + return ret; +} + +void ManyBoneIK3D::set_constraint_name_at_index(int32_t p_index, String p_name) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + constraint_names.write[p_index] = p_name; + set_dirty(); +} + Vector> ManyBoneIK3D::get_segmented_skeletons() { return segmented_skeletons; } @@ -379,7 +684,7 @@ void ManyBoneIK3D::_process_modification() { if (segmented_skeleton.is_null()) { continue; } - segmented_skeleton->segment_solver(bone_damp, get_default_damp(), false, i, get_iterations_per_frame()); + segmented_skeleton->segment_solver(bone_damp, get_default_damp(), get_constraint_mode(), i, get_iterations_per_frame()); } } _update_skeleton_bones_transform(); @@ -423,6 +728,28 @@ void ManyBoneIK3D::set_dirty() { is_dirty = true; } +int32_t ManyBoneIK3D::find_constraint(String p_string) const { + for (int32_t constraint_i = 0; constraint_i < constraint_count; constraint_i++) { + if (get_constraint_name(constraint_i) == p_string) { + return constraint_i; + } + } + return -1; +} + +void ManyBoneIK3D::remove_pin_at_index(int32_t p_index) { + ERR_FAIL_INDEX(p_index, constraint_count); + + constraint_names.remove_at(p_index); + kusudama_open_cone_count.remove_at(p_index); + kusudama_open_cones.remove_at(p_index); + joint_twist.remove_at(p_index); + + constraint_count--; + + set_dirty(); +} + void ManyBoneIK3D::_set_bone_count(int32_t p_count) { bone_damp.resize(p_count); for (int32_t bone_i = p_count; bone_i-- > bone_count;) { @@ -441,6 +768,143 @@ Vector> ManyBoneIK3D::get_bone_list() const { return bone_list; } +void ManyBoneIK3D::set_direction_transform_of_bone(int32_t p_index, Transform3D p_transform) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + if (!get_skeleton()) { + return; + } + String bone_name = constraint_names[p_index]; + int32_t bone_index = get_skeleton()->find_bone(bone_name); + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(bone_index); + if (ik_bone.is_null() || ik_bone->get_constraint().is_null()) { + continue; + } + if (ik_bone->get_bone_direction_transform().is_null()) { + continue; + } + ik_bone->get_bone_direction_transform()->set_transform(p_transform); + break; + } +} + +Transform3D ManyBoneIK3D::get_direction_transform_of_bone(int32_t p_index) const { + if (p_index < 0 || p_index >= constraint_names.size() || get_skeleton() == nullptr) { + return Transform3D(); + } + + String bone_name = constraint_names[p_index]; + int32_t bone_index = get_skeleton()->find_bone(bone_name); + for (const Ref &segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(bone_index); + if (ik_bone.is_null() || ik_bone->get_constraint().is_null()) { + continue; + } + return ik_bone->get_bone_direction_transform()->get_transform(); + } + return Transform3D(); +} + +Transform3D ManyBoneIK3D::get_orientation_transform_of_constraint(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, constraint_names.size(), Transform3D()); + String bone_name = constraint_names[p_index]; + if (!segmented_skeletons.size()) { + return Transform3D(); + } + if (!get_skeleton()) { + return Transform3D(); + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + return ik_bone->get_constraint_orientation_transform()->get_transform(); + } + return Transform3D(); +} + +void ManyBoneIK3D::set_orientation_transform_of_constraint(int32_t p_index, Transform3D p_transform) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + String bone_name = constraint_names[p_index]; + if (!get_skeleton()) { + return; + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + ik_bone->get_constraint_orientation_transform()->set_transform(p_transform); + break; + } +} + +Transform3D ManyBoneIK3D::get_twist_transform_of_constraint(int32_t p_index) const { + ERR_FAIL_INDEX_V(p_index, constraint_names.size(), Transform3D()); + String bone_name = constraint_names[p_index]; + if (!segmented_skeletons.size()) { + return Transform3D(); + } + if (!get_skeleton()) { + return Transform3D(); + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + return ik_bone->get_constraint_twist_transform()->get_transform(); + } + return Transform3D(); +} + +void ManyBoneIK3D::set_twist_transform_of_constraint(int32_t p_index, Transform3D p_transform) { + ERR_FAIL_INDEX(p_index, constraint_names.size()); + String bone_name = constraint_names[p_index]; + if (!get_skeleton()) { + return; + } + for (Ref segmented_skeleton : segmented_skeletons) { + if (segmented_skeleton.is_null()) { + continue; + } + Ref ik_bone = segmented_skeleton->get_ik_bone(get_skeleton()->find_bone(bone_name)); + if (ik_bone.is_null()) { + continue; + } + if (ik_bone->get_constraint().is_null()) { + continue; + } + ik_bone->get_constraint_twist_transform()->set_transform(p_transform); + break; + } +} + bool ManyBoneIK3D::get_pin_enabled(int32_t p_effector_index) const { ERR_FAIL_INDEX_V(p_effector_index, pins.size(), false); Ref effector_template = pins[p_effector_index]; @@ -450,6 +914,44 @@ bool ManyBoneIK3D::get_pin_enabled(int32_t p_effector_index) const { return !effector_template->get_target_node().is_empty(); } +void ManyBoneIK3D::register_skeleton() { + if (!get_pin_count() && !get_constraint_count()) { + reset_constraints(); + } + set_dirty(); +} + +void ManyBoneIK3D::reset_constraints() { + Skeleton3D *skeleton = get_skeleton(); + if (skeleton) { + int32_t saved_pin_count = get_pin_count(); + set_pin_count(0); + set_pin_count(saved_pin_count); + int32_t saved_constraint_count = constraint_names.size(); + _set_constraint_count(0); + _set_constraint_count(saved_constraint_count); + _set_bone_count(0); + _set_bone_count(saved_constraint_count); + } + set_dirty(); +} + +bool ManyBoneIK3D::get_constraint_mode() const { + return is_constraint_mode; +} + +void ManyBoneIK3D::set_constraint_mode(bool p_enabled) { + is_constraint_mode = p_enabled; +} + +int32_t ManyBoneIK3D::get_ui_selected_bone() const { + return ui_selected_bone; +} + +void ManyBoneIK3D::set_ui_selected_bone(int32_t p_ui_selected_bone) { + ui_selected_bone = p_ui_selected_bone; +} + void ManyBoneIK3D::set_stabilization_passes(int32_t p_passes) { stabilize_passes = p_passes; set_dirty(); @@ -467,6 +969,17 @@ Ref ManyBoneIK3D::get_godot_skeleton_transform() { return godot_skeleton_transform; } +void ManyBoneIK3D::add_constraint() { + int32_t old_count = constraint_count; + _set_constraint_count(constraint_count + 1); + constraint_names.write[old_count] = String(); + kusudama_open_cone_count.write[old_count] = 0; + kusudama_open_cones.write[old_count].resize(1); + kusudama_open_cones.write[old_count].write[0] = Vector4(0, 1, 0, Math_PI); + joint_twist.write[old_count] = Vector2(0, Math_PI); + set_dirty(); +} + int32_t ManyBoneIK3D::find_pin(String p_string) const { for (int32_t pin_i = 0; pin_i < pin_count; pin_i++) { if (get_pin_bone_name(pin_i) == p_string) { @@ -518,6 +1031,37 @@ void ManyBoneIK3D::_bone_list_changed() { for (Ref &ik_bone_3d : bone_list) { ik_bone_3d->update_default_bone_direction_transform(skeleton); } + for (int constraint_i = 0; constraint_i < constraint_count; ++constraint_i) { + String bone = constraint_names[constraint_i]; + BoneId bone_id = skeleton->find_bone(bone); + for (Ref &ik_bone_3d : bone_list) { + if (ik_bone_3d->get_bone_id() != bone_id) { + continue; + } + Ref constraint; + constraint.instantiate(); + constraint->enable_orientational_limits(); + + int32_t cone_count = kusudama_open_cone_count[constraint_i]; + const Vector &cones = kusudama_open_cones[constraint_i]; + for (int32_t cone_i = 0; cone_i < cone_count; ++cone_i) { + const Vector4 &cone = cones[cone_i]; + Ref new_cone; + new_cone.instantiate(); + new_cone->set_attached_to(constraint); + new_cone->set_radius(MAX(1.0e-38, cone.w)); + new_cone->set_control_point(Vector3(cone.x, cone.y, cone.z).normalized()); + constraint->add_open_cone(new_cone); + } + + const Vector2 axial_limit = get_joint_twist(constraint_i); + constraint->enable_axial_limits(); + constraint->set_axial_limits(axial_limit.x, axial_limit.y); + ik_bone_3d->add_constraint(constraint); + constraint->_update_constraint(ik_bone_3d->get_constraint_twist_transform()); + break; + } + } } void ManyBoneIK3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) { diff --git a/modules/many_bone_ik/src/many_bone_ik_3d.h b/modules/many_bone_ik/src/many_bone_ik_3d.h index 9c349992e4be..0129e33a67fc 100644 --- a/modules/many_bone_ik/src/many_bone_ik_3d.h +++ b/modules/many_bone_ik/src/many_bone_ik_3d.h @@ -36,7 +36,6 @@ #include "core/math/vector3.h" #include "core/object/ref_counted.h" #include "ik_bone_3d.h" -#include "ik_bone_segment_3d.h" #include "ik_effector_template_3d.h" #include "math/ik_node_3d.h" #include "scene/3d/skeleton_3d.h" @@ -46,15 +45,18 @@ class ManyBoneIK3DState; class ManyBoneIK3D : public SkeletonModifier3D { GDCLASS(ManyBoneIK3D, SkeletonModifier3D); + bool is_constraint_mode = false; NodePath skeleton_path; Vector> segmented_skeletons; - int32_t pin_count = 0, bone_count = 0; + int32_t constraint_count = 0, pin_count = 0, bone_count = 0; Vector constraint_names; Vector> pins; Vector> bone_list; + Vector joint_twist; Vector bone_damp; Vector> kusudama_open_cones; Vector kusudama_open_cone_count; + float MAX_KUSUDAMA_OPEN_CONES = 10; int32_t iterations_per_frame = 15; float default_damp = Math::deg_to_rad(5.0f); Ref godot_skeleton_transform; @@ -68,6 +70,8 @@ class ManyBoneIK3D : public SkeletonModifier3D { void _update_ik_bones_transform(); void _update_skeleton_bones_transform(); Vector> _get_bone_effectors() const; + void set_constraint_name_at_index(int32_t p_index, String p_name); + void _set_constraint_count(int32_t p_count); void _remove_pin(int32_t p_index); void _set_bone_count(int32_t p_count); void _set_pin_root_bone(int32_t p_pin_index, const String &p_root_bone); @@ -89,11 +93,18 @@ class ManyBoneIK3D : public SkeletonModifier3D { bool get_pin_target_fixed(int32_t p_effector_index); void set_state(Ref p_state); Ref get_state() const; + void add_constraint(); void set_stabilization_passes(int32_t p_passes); int32_t get_stabilization_passes(); Transform3D get_godot_skeleton_transform_inverse(); Ref get_godot_skeleton_transform(); + void set_ui_selected_bone(int32_t p_ui_selected_bone); + int32_t get_ui_selected_bone() const; + void set_constraint_mode(bool p_enabled); + bool get_constraint_mode() const; bool get_pin_enabled(int32_t p_effector_index) const; + void register_skeleton(); + void reset_constraints(); Vector> get_bone_list() const; Vector> get_segmented_skeletons(); float get_iterations_per_frame() const; @@ -101,6 +112,7 @@ class ManyBoneIK3D : public SkeletonModifier3D { void queue_print_skeleton(); int32_t get_pin_count() const; void set_pin_count(int32_t p_pin_count); + void remove_pin_at_index(int32_t p_index); void set_pin_bone_name(int32_t p_pin_index, const String &p_bone); StringName get_pin_bone_name(int32_t p_effector_index) const; void set_pin_node_path(int32_t p_effector_index, NodePath p_node_path); @@ -116,8 +128,28 @@ class ManyBoneIK3D : public SkeletonModifier3D { float get_pin_motion_propagation_factor(int32_t p_effector_index) const; real_t get_default_damp() const; void set_default_damp(float p_default_damp); + int32_t find_constraint(String p_string) const; int32_t find_pin(String p_string) const; + int32_t get_constraint_count() const; + StringName get_constraint_name(int32_t p_index) const; + void set_twist_transform_of_constraint(int32_t p_index, Transform3D p_transform); + Transform3D get_twist_transform_of_constraint(int32_t p_index) const; + void set_orientation_transform_of_constraint(int32_t p_index, Transform3D p_transform); + Transform3D get_orientation_transform_of_constraint(int32_t p_index) const; + void set_direction_transform_of_bone(int32_t p_index, Transform3D p_transform); + Transform3D get_direction_transform_of_bone(int32_t p_index) const; + Vector2 get_joint_twist(int32_t p_index) const; + void set_joint_twist(int32_t p_index, Vector2 p_twist); + void set_kusudama_open_cone(int32_t p_bone, int32_t p_index, + Vector3 p_center, float p_radius); + Vector3 get_kusudama_open_cone_center(int32_t p_constraint_index, int32_t p_index) const; + float get_kusudama_open_cone_radius(int32_t p_constraint_index, int32_t p_index) const; + int32_t get_kusudama_open_cone_count(int32_t p_constraint_index) const; int32_t get_bone_count() const; + void set_kusudama_twist_from_to(int32_t p_index, float from, float to); + void set_kusudama_open_cone_count(int32_t p_constraint_index, int32_t p_count); + void set_kusudama_open_cone_center(int32_t p_constraint_index, int32_t p_index, Vector3 p_center); + void set_kusudama_open_cone_radius(int32_t p_constraint_index, int32_t p_index, float p_radius); ManyBoneIK3D(); ~ManyBoneIK3D(); void set_dirty(); diff --git a/modules/constraint_ik/tests/test_ik_kusudama_3d.h b/modules/many_bone_ik/tests/test_ik_kusudama_3d.h similarity index 99% rename from modules/constraint_ik/tests/test_ik_kusudama_3d.h rename to modules/many_bone_ik/tests/test_ik_kusudama_3d.h index 43b98a02371e..6f16190bf59b 100644 --- a/modules/constraint_ik/tests/test_ik_kusudama_3d.h +++ b/modules/many_bone_ik/tests/test_ik_kusudama_3d.h @@ -30,7 +30,7 @@ #ifndef TEST_IK_KUSUDAMA_3D_H #define TEST_IK_KUSUDAMA_3D_H -#include "modules/constraint_ik/src/ik_kusudama_3d.h" +#include "modules/many_bone_ik/src/ik_kusudama_3d.h" #include "tests/test_macros.h" namespace TestIKKusudama3D {