diff --git a/modules/many_bone_ik/.github/ISSUE_TEMPLATE/bug_report.md b/modules/many_bone_ik/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 000000000000..9b77ea713f98 --- /dev/null +++ b/modules/many_bone_ik/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,40 @@ +--- +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/many_bone_ik/.github/ISSUE_TEMPLATE/feature_request.md b/modules/many_bone_ik/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 000000000000..2bc5d5f71186 --- /dev/null +++ b/modules/many_bone_ik/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,19 @@ +--- +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/many_bone_ik/.gitignore b/modules/many_bone_ik/.gitignore new file mode 100644 index 000000000000..3946cc96e540 --- /dev/null +++ b/modules/many_bone_ik/.gitignore @@ -0,0 +1,380 @@ +# 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/many_bone_ik/.gitrepo b/modules/many_bone_ik/.gitrepo new file mode 100644 index 000000000000..335e0efb84b6 --- /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 = ddc539e5153214d1f74a11d3316eb6679357e261 + parent = a66d6293c8c00b28dcde529e105741bdf0d82ae3 + method = merge + cmdver = 0.4.6 diff --git a/modules/many_bone_ik/LICENSE b/modules/many_bone_ik/LICENSE new file mode 100644 index 000000000000..24a6b933f27a --- /dev/null +++ b/modules/many_bone_ik/LICENSE @@ -0,0 +1,23 @@ +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/many_bone_ik/SCsub b/modules/many_bone_ik/SCsub new file mode 100644 index 000000000000..715fb0101706 --- /dev/null +++ b/modules/many_bone_ik/SCsub @@ -0,0 +1,15 @@ +#!/usr/bin/env python + +Import("env") + +env_many_bone_ik = env.Clone() +env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik"]) +env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik/src/math"]) +env_many_bone_ik.Prepend(CPPPATH=["#modules/many_bone_ik/src"]) +env_many_bone_ik.add_source_files(env.modules_sources, "constraints/*.cpp") +env_many_bone_ik.add_source_files(env.modules_sources, "src/math/*.cpp") +env_many_bone_ik.add_source_files(env.modules_sources, "src/*.cpp") +env_many_bone_ik.add_source_files(env.modules_sources, "*.cpp") + +if env.editor_build: + env_many_bone_ik.add_source_files(env.modules_sources, "editor/*.cpp") diff --git a/modules/many_bone_ik/config.py b/modules/many_bone_ik/config.py new file mode 100644 index 000000000000..b27d55d5d10c --- /dev/null +++ b/modules/many_bone_ik/config.py @@ -0,0 +1,24 @@ +def can_build(env, platform): + return not env["disable_3d"] + + +def configure(env): + pass + + +def get_doc_classes(): + return [ + "ManyBoneIK3D", + "IKBone3D", + "IKEffector3D", + "IKBoneSegment3D", + "IKEffectorTemplate3D", + "IKKusudama3D", + "IKRay3D", + "IKNode3D", + "IKLimitCone3D", + ] + + +def get_doc_path(): + return "doc_classes" diff --git a/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CB22CAE13E99E3E24B.png b/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CB22CAE13E99E3E24B.png new file mode 100644 index 000000000000..8db02055081d Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CB22CAE13E99E3E24B.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CB360C51B9B2145043.png b/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CB360C51B9B2145043.png new file mode 100644 index 000000000000..a565d772d250 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CB360C51B9B2145043.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CBF393B4C3206D97F3.png b/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CBF393B4C3206D97F3.png new file mode 100644 index 000000000000..8be46c6e6ebc Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/1000000000000139000000CBF393B4C3206D97F3.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/100000000000016C000002AAA2059A0E43B2B468.png b/modules/many_bone_ik/design_docs/Pictures/100000000000016C000002AAA2059A0E43B2B468.png new file mode 100644 index 000000000000..9a8fa6af5bb3 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/100000000000016C000002AAA2059A0E43B2B468.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E1025A91C2576C186.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E1025A91C2576C186.png new file mode 100644 index 000000000000..0daccbefc874 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E1025A91C2576C186.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E173E86C323891A74.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E173E86C323891A74.png new file mode 100644 index 000000000000..e45691aee337 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E173E86C323891A74.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E6996B4A5DF706332.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E6996B4A5DF706332.png new file mode 100644 index 000000000000..40a36acf88a8 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E6996B4A5DF706332.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E832AFC8FF38BF53F.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E832AFC8FF38BF53F.png new file mode 100644 index 000000000000..fe5c87f9b02f Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E832AFC8FF38BF53F.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E845771BD15F39314.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E845771BD15F39314.png new file mode 100644 index 000000000000..871a661f3aa8 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E845771BD15F39314.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E853FACED42A51700.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E853FACED42A51700.png new file mode 100644 index 000000000000..e53044de435e Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E853FACED42A51700.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E9D87E7B297A94C9F.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E9D87E7B297A94C9F.png new file mode 100644 index 000000000000..d18d3a42cc54 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018E9D87E7B297A94C9F.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EA6B7A87AEB9D9F40.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EA6B7A87AEB9D9F40.png new file mode 100644 index 000000000000..f77f5b60d8d0 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EA6B7A87AEB9D9F40.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EBC567E962B9B4C87.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EBC567E962B9B4C87.png new file mode 100644 index 000000000000..2919df40c0ce Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EBC567E962B9B4C87.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018ECB3181C74F9EDC35.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018ECB3181C74F9EDC35.png new file mode 100644 index 000000000000..d30d4927b56c Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018ECB3181C74F9EDC35.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018ED76B18851DB9DC34.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018ED76B18851DB9DC34.png new file mode 100644 index 000000000000..9373407d7bb5 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018ED76B18851DB9DC34.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EDE8A6B18141900B0.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EDE8A6B18141900B0.png new file mode 100644 index 000000000000..efb7b31856f7 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EDE8A6B18141900B0.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EDF01F2A95A1519D7.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EDF01F2A95A1519D7.png new file mode 100644 index 000000000000..c11b75c6c1be Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EDF01F2A95A1519D7.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EE63F4ECE1A82B606.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EE63F4ECE1A82B606.png new file mode 100644 index 000000000000..196f99804799 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EE63F4ECE1A82B606.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EEE6FC1E017012EDA.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EEE6FC1E017012EDA.png new file mode 100644 index 000000000000..66ab6e24eb43 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002070000018EEE6FC1E017012EDA.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002C90000027876C8370C9358FB94.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002C90000027876C8370C9358FB94.png new file mode 100644 index 000000000000..50e9f91d321a Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002C90000027876C8370C9358FB94.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002C900000278AEBB2DF17488D353.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002C900000278AEBB2DF17488D353.png new file mode 100644 index 000000000000..4618a5411f08 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002C900000278AEBB2DF17488D353.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000002C900000278EEE925F5104E07BE.png b/modules/many_bone_ik/design_docs/Pictures/10000000000002C900000278EEE925F5104E07BE.png new file mode 100644 index 000000000000..a2263757b5ba Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000002C900000278EEE925F5104E07BE.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000000000004000000040014FDC3377E0AD7E4.png b/modules/many_bone_ik/design_docs/Pictures/10000000000004000000040014FDC3377E0AD7E4.png new file mode 100644 index 000000000000..9acbcd9dd982 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000000000004000000040014FDC3377E0AD7E4.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/1000000000000400000004007DA1871E5E883934.png b/modules/many_bone_ik/design_docs/Pictures/1000000000000400000004007DA1871E5E883934.png new file mode 100644 index 000000000000..ca6ceecb5be6 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/1000000000000400000004007DA1871E5E883934.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/10000201000002020000018EF75E743EDC6A705E.png b/modules/many_bone_ik/design_docs/Pictures/10000201000002020000018EF75E743EDC6A705E.png new file mode 100644 index 000000000000..a949b58c73f1 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/10000201000002020000018EF75E743EDC6A705E.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/1000020100000286000001D10AA6EC3093AC56D5.png b/modules/many_bone_ik/design_docs/Pictures/1000020100000286000001D10AA6EC3093AC56D5.png new file mode 100644 index 000000000000..86258c569625 Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/1000020100000286000001D10AA6EC3093AC56D5.png differ diff --git a/modules/many_bone_ik/design_docs/Pictures/100002010000030D0000026B7F70BE2E85BEF4BE.png b/modules/many_bone_ik/design_docs/Pictures/100002010000030D0000026B7F70BE2E85BEF4BE.png new file mode 100644 index 000000000000..f473a770fa7b Binary files /dev/null and b/modules/many_bone_ik/design_docs/Pictures/100002010000030D0000026B7F70BE2E85BEF4BE.png differ diff --git a/modules/many_bone_ik/design_docs/readme.md b/modules/many_bone_ik/design_docs/readme.md new file mode 100644 index 000000000000..a3105edae229 --- /dev/null +++ b/modules/many_bone_ik/design_docs/readme.md @@ -0,0 +1,555 @@ +# 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/many_bone_ik/doc_classes/IKBone3D.xml b/modules/many_bone_ik/doc_classes/IKBone3D.xml new file mode 100644 index 000000000000..d7eb00fa46f1 --- /dev/null +++ b/modules/many_bone_ik/doc_classes/IKBone3D.xml @@ -0,0 +1,50 @@ + + + + A class representing a 3D bone in an Inverse Kinematics system. + + + The IKBone3D 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 IKNode3D object representing the orientation transform of the constraint. + + + + + + Returns the IKNode3D object representing the twist transform of the constraint. + + + + + + Returns the IKEffector3D object representing the pin of the bone. + + + + + + Returns true if the bone is pinned, false otherwise. + + + + + + + Sets the IKEffector3D object representing the pin of the bone. + + + + diff --git a/modules/many_bone_ik/doc_classes/IKBoneSegment3D.xml b/modules/many_bone_ik/doc_classes/IKBoneSegment3D.xml new file mode 100644 index 000000000000..0773e79ed95e --- /dev/null +++ b/modules/many_bone_ik/doc_classes/IKBoneSegment3D.xml @@ -0,0 +1,26 @@ + + + + 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 IKBone3D object associated with the given bone index. + + + + + + Returns true if the bone segment is pinned, false otherwise. + + + + diff --git a/modules/many_bone_ik/doc_classes/IKEffector3D.xml b/modules/many_bone_ik/doc_classes/IKEffector3D.xml new file mode 100644 index 000000000000..58ce01e189d0 --- /dev/null +++ b/modules/many_bone_ik/doc_classes/IKEffector3D.xml @@ -0,0 +1,36 @@ + + + + A class representing a 3D effector in an Inverse Kinematics system. + + + The IKEffector3D 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. + + + + + + + + Returns the NodePath of the target node for this effector. The target node is the node that the effector aims to reach. + + + + + + + + Sets the target node for this effector using a Skeleton3D and a NodePath. The effector will aim to reach this target node. + + + + + + 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/many_bone_ik/doc_classes/IKEffectorTemplate3D.xml b/modules/many_bone_ik/doc_classes/IKEffectorTemplate3D.xml new file mode 100644 index 000000000000..e0c695472e01 --- /dev/null +++ b/modules/many_bone_ik/doc_classes/IKEffectorTemplate3D.xml @@ -0,0 +1,26 @@ + + + + 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 NodePath of the target node that the effector aims to reach. + + + 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/many_bone_ik/doc_classes/IKKusudama3D.xml b/modules/many_bone_ik/doc_classes/IKKusudama3D.xml new file mode 100644 index 000000000000..d1ab073a8542 --- /dev/null +++ b/modules/many_bone_ik/doc_classes/IKKusudama3D.xml @@ -0,0 +1,26 @@ + + + + Kusudamas are sequential collections of reach cones that form a path through their tangents. + + + A Kusudama represents a ball with multiple reach-cones protruding from it. These reach cones are arranged in sequence, creating an automatic smooth path from one cone to another. The term 'Kusudama' originates from Japanese, referring to a ball with multiple cones protruding from it. + + + + + + + + This method returns an array of limit cones associated with the Kusudama. + + + + + + + This method sets the limit cones associated with the Kusudama. + + + + diff --git a/modules/many_bone_ik/doc_classes/IKLimitCone3D.xml b/modules/many_bone_ik/doc_classes/IKLimitCone3D.xml new file mode 100644 index 000000000000..05b2d596dff6 --- /dev/null +++ b/modules/many_bone_ik/doc_classes/IKLimitCone3D.xml @@ -0,0 +1,11 @@ + + + + A 3D cone used to open the rotation of a ball-and-socket joint in inverse kinematics calculations. + + + A open cone is a cone freeing the rotation of a ball-and-socket joint. A open cone is defined as a vector pointing in the direction which the cone is opening, and a radius (in radians) representing how much the cone is opening up. + + + + diff --git a/modules/many_bone_ik/doc_classes/IKNode3D.xml b/modules/many_bone_ik/doc_classes/IKNode3D.xml new file mode 100644 index 000000000000..969341bd8f3c --- /dev/null +++ b/modules/many_bone_ik/doc_classes/IKNode3D.xml @@ -0,0 +1,87 @@ + + + + A 3D node used for inverse kinematics calculations. + + + The IKNode3D 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/many_bone_ik/doc_classes/IKRay3D.xml b/modules/many_bone_ik/doc_classes/IKRay3D.xml new file mode 100644 index 000000000000..c2ef554912dd --- /dev/null +++ b/modules/many_bone_ik/doc_classes/IKRay3D.xml @@ -0,0 +1,35 @@ + + + + A class representing a 3D ray in an Inverse Kinematics system. + + + The IKRay3D class represents a 3D ray in an Inverse Kinematics (IK) system. It provides methods to get the heading of the ray, check if the ray intersects a plane, and get a scaled projection of the ray. + + + + + + + + Returns the heading of the ray as a Vector3. + + + + + + + + + Checks if the ray intersects a plane defined by three points (a, b, c). Returns the intersection point as a Vector3, or null if there is no intersection. + + + + + + + Returns the scaled projection of the ray onto the input vector. + + + + diff --git a/modules/many_bone_ik/doc_classes/ManyBoneIK3D.xml b/modules/many_bone_ik/doc_classes/ManyBoneIK3D.xml new file mode 100644 index 000000000000..dab81b413d29 --- /dev/null +++ b/modules/many_bone_ik/doc_classes/ManyBoneIK3D.xml @@ -0,0 +1,298 @@ + + + + A general inverse kinematics system with constraints. + + + The ManyBoneIK3D 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. + + + + + + + + + + + + Returns the total number of pins in the IK system. + + + + + + + Returns the direction priorities of the pin at the specified index. + + + + + + + Returns whether the pin at the specified index is enabled or not. + + + + + + + Returns the passthrough factor of the pin at the specified index. + + + + + + + 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. + + + + + + + + + + + + + + + + + + + + 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 name of the bone at the specified pin index. + + + + + + + + + + + + + + + + + + + + + + + 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. + + + + + + + + + + + + + + + + + + + + + Sets the direction priorities of the pin at the specified index. + + + + + + + + Sets the passthrough factor of the pin at the specified index. + + + + + + + + Sets the weight of the pin 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/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp b/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp new file mode 100644 index 000000000000..5481406c28e2 --- /dev/null +++ b/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.cpp @@ -0,0 +1,600 @@ +/**************************************************************************/ +/* many_bone_ik_3d_gizmo_plugin.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 "core/math/transform_3d.h" +#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" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/surface_tool.h" + +#include "../src/ik_bone_3d.h" +#include "../src/ik_kusudama_3d.h" +#include "../src/many_bone_ik_3d.h" +#include "many_bone_ik_3d_gizmo_plugin.h" + +#ifdef TOOLS_ENABLED +#include "editor/editor_node.h" +#include "editor/editor_undo_redo_manager.h" +#endif + +void ManyBoneIK3DGizmoPlugin::_bind_methods() { + ClassDB::bind_method(D_METHOD("_get_gizmo_name"), &ManyBoneIK3DGizmoPlugin::get_gizmo_name); +} + +bool ManyBoneIK3DGizmoPlugin::has_gizmo(Node3D *p_spatial) { + return cast_to(p_spatial); +} + +String ManyBoneIK3DGizmoPlugin::get_gizmo_name() const { + return "ManyBoneIK3D"; +} + +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; + } + if (handles_mesh_instance && !handles_mesh_instance->is_inside_tree()) { + skeleton->call_deferred("add_child", handles_mesh_instance); + handles_mesh_instance->set_skeleton_path(NodePath("")); + } + int selected = -1; + Skeleton3DEditor *se = Skeleton3DEditor::get_singleton(); + if (se) { + selected = se->get_selected_bone(); + } + Color selected_bone_color = EDITOR_GET("editors/3d_gizmos/gizmo_colors/selected_bone"); + + int current_bone_index = 0; + Vector bones_to_process = skeleton->get_parentless_bones(); + + while (bones_to_process.size() > current_bone_index) { + int current_bone_idx = bones_to_process[current_bone_index]; + + Color current_bone_color = (current_bone_idx == selected) ? selected_bone_color : bone_color; + + 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); + if (ik_bone.is_null() || ik_bone->get_constraint().is_null()) { + continue; + } + create_gizmo_mesh(current_bone_idx, ik_bone, p_gizmo, current_bone_color, skeleton, many_bone_ik); + } + + current_bone_index++; + + Vector child_bones_vector; + child_bones_vector = skeleton->get_bone_children(current_bone_idx); + int child_bones_size = child_bones_vector.size(); + for (int i = 0; i < child_bones_size; i++) { + // Something wrong. + if (child_bones_vector[i] < 0) { + continue; + } + // Add the bone's children to the list of bones to be processed. + bones_to_process.push_back(child_bones_vector[i]); + } + } +} + +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; + } + const TypedArray &open_cones = ik_kusudama->get_open_cones(); + if (!open_cones.size()) { + return; + } + BoneId parent_idx = many_bone_ik_skeleton->get_bone_parent(current_bone_idx); + LocalVector bones; + LocalVector weights; + bones.resize(4); + weights.resize(4); + for (int i = 0; i < 4; i++) { + bones[i] = 0; + weights[i] = 0; + } + bones[0] = parent_idx; + weights[0] = 1; + + Transform3D constraint_relative_to_the_skeleton = p_many_bone_ik->get_relative_transform(p_many_bone_ik->get_owner()).affine_inverse() * many_bone_ik_skeleton->get_relative_transform(many_bone_ik_skeleton->get_owner()) * p_many_bone_ik->get_godot_skeleton_transform_inverse() * ik_bone->get_constraint_orientation_transform()->get_global_transform(); + PackedFloat32Array kusudama_open_cones; + Ref kusudama = ik_bone->get_constraint(); + for (int32_t cone_i = 0; cone_i < open_cones.size(); cone_i++) { + Ref open_cone = open_cones[cone_i]; + Vector3 control_point = open_cone->get_control_point(); + PackedFloat32Array new_kusudama_open_cones; + new_kusudama_open_cones.resize(4 * 3); + new_kusudama_open_cones.fill(0.0f); + new_kusudama_open_cones.write[0] = control_point.x; + new_kusudama_open_cones.write[1] = control_point.y; + new_kusudama_open_cones.write[2] = control_point.z; + float radius = open_cone->get_radius(); + new_kusudama_open_cones.write[3] = radius; + + Vector3 tangent_center_1 = open_cone->get_tangent_circle_center_next_1(); + new_kusudama_open_cones.write[4] = tangent_center_1.x; + new_kusudama_open_cones.write[5] = tangent_center_1.y; + new_kusudama_open_cones.write[6] = tangent_center_1.z; + float tangent_radius = open_cone->get_tangent_circle_radius_next(); + new_kusudama_open_cones.write[7] = tangent_radius; + + Vector3 tangent_center_2 = open_cone->get_tangent_circle_center_next_2(); + new_kusudama_open_cones.write[8] = tangent_center_2.x; + new_kusudama_open_cones.write[9] = tangent_center_2.y; + new_kusudama_open_cones.write[10] = tangent_center_2.z; + new_kusudama_open_cones.write[11] = tangent_radius; + + kusudama_open_cones.append_array(new_kusudama_open_cones); + } + if (current_bone_idx >= many_bone_ik_skeleton->get_bone_count()) { + return; + } + if (current_bone_idx == -1) { + return; + } + if (parent_idx >= many_bone_ik_skeleton->get_bone_count()) { + return; + } + if (parent_idx <= -1) { + return; + } + // Code copied from the SphereMesh. + int rings = 8; + + int i = 0, j = 0, prevrow = 0, thisrow = 0, point = 0; + float x, y, z; + + Vector points; + Vector normals; + Vector indices; + point = 0; + + thisrow = 0; + prevrow = 0; + for (j = 0; j <= (rings + 1); j++) { + int radial_segments = 8; + float v = j; + float w; + + v /= (rings + 1); + w = sin(Math_PI * v); + y = cos(Math_PI * v); + + for (i = 0; i <= radial_segments; i++) { + float u = i; + u /= radial_segments; + + x = sin(u * Math_TAU); + z = cos(u * Math_TAU); + + Vector3 p = Vector3(x * w, y, z * w) * 0.02f; + points.push_back(p); + Vector3 normal = Vector3(x * w, y, z * w); + normals.push_back(normal.normalized()); + point++; + + if (i > 0 && j > 0) { + indices.push_back(prevrow + i - 1); + indices.push_back(prevrow + i); + indices.push_back(thisrow + i - 1); + + indices.push_back(prevrow + i); + indices.push_back(thisrow + i); + indices.push_back(thisrow + i - 1); + }; + }; + + prevrow = thisrow; + thisrow = point; + } + if (!indices.size()) { + return; + } + Ref surface_tool; + surface_tool.instantiate(); + surface_tool->begin(Mesh::PRIMITIVE_TRIANGLES); + const int32_t MESH_CUSTOM_0 = 0; + surface_tool->set_custom_format(MESH_CUSTOM_0, SurfaceTool::CustomFormat::CUSTOM_RGBA_HALF); + for (int32_t point_i = 0; point_i < points.size(); point_i++) { + surface_tool->set_bones(bones); + surface_tool->set_weights(weights); + Color c; + c.r = normals[point_i].x; + c.g = normals[point_i].y; + c.b = normals[point_i].z; + c.a = 0; + surface_tool->set_custom(MESH_CUSTOM_0, c); + surface_tool->set_normal(normals[point_i]); + surface_tool->add_vertex(points[point_i]); + } + for (int32_t index_i : indices) { + surface_tool->add_index(index_i); + } + Ref kusudama_material; + kusudama_material.instantiate(); + kusudama_material->set_shader(kusudama_shader); + kusudama_material->set_shader_parameter("cone_sequence", kusudama_open_cones); + int32_t cone_count = kusudama->get_open_cones().size(); + kusudama_material->set_shader_parameter("cone_count", cone_count); + kusudama_material->set_shader_parameter("kusudama_color", current_bone_color); + p_gizmo->add_mesh( + surface_tool->commit(Ref(), RS::ARRAY_CUSTOM_RGBA_HALF << RS::ARRAY_FORMAT_CUSTOM0_SHIFT), + kusudama_material, constraint_relative_to_the_skeleton); +} + +int32_t ManyBoneIK3DGizmoPlugin::get_priority() const { + return -1; +} + +EditorPluginManyBoneIK::EditorPluginManyBoneIK() { + Ref many_bone_ik_gizmo_plugin; + many_bone_ik_gizmo_plugin.instantiate(); + Node3DEditor::get_singleton()->add_gizmo_plugin(many_bone_ik_gizmo_plugin); +} + +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) { + return -1; + } + + if (Node3DEditor::get_singleton()->get_tool_mode() != Node3DEditor::TOOL_MODE_SELECT) { + return -1; + } + + // Select bone. + real_t grab_threshold = 4 * EDSCALE; + Vector3 ray_from = p_camera->get_global_transform().origin; + Transform3D gt = skeleton->get_global_transform(); + int closest_idx = -1; + real_t closest_dist = 1e10; + const int bone_count = skeleton->get_bone_count(); + + for (int i = 0; i < bone_count; i++) { + Vector3 joint_pos_3d = gt.xform(skeleton->get_bone_global_pose(i).origin); + Vector2 joint_pos_2d = p_camera->unproject_position(joint_pos_3d); + real_t dist_3d = ray_from.distance_to(joint_pos_3d); + real_t dist_2d = p_point.distance_to(joint_pos_2d); + if (dist_2d < grab_threshold && dist_3d < closest_dist) { + closest_dist = dist_3d; + closest_idx = i; + } + } + + if (closest_idx >= 0) { + many_bone_ik->set_ui_selected_bone(closest_idx); + return closest_idx; + } + + many_bone_ik->set_ui_selected_bone(-1); + return -1; +} + +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() * + skeleton->get_relative_transform(skeleton->get_owner()) * many_bone_ik->get_godot_skeleton_transform_inverse(); + return constraint_relative_to_the_skeleton * skeleton->get_bone_global_pose(p_id); +} + +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; + int parent_idx = skeleton->get_bone_parent(p_id); + if (parent_idx >= 0) { + original_to_local = skeleton->get_bone_global_pose(parent_idx); + } + Basis to_local = original_to_local.get_basis().inverse(); + + // Prepare transform. + Transform3D t; + + // Basis. + t.basis = to_local * p_transform.get_basis(); + + Transform3D constraint_relative_to_the_skeleton = many_bone_ik->get_relative_transform(many_bone_ik->get_owner()).affine_inverse() * + skeleton->get_relative_transform(skeleton->get_owner()) * many_bone_ik->get_godot_skeleton_transform_inverse(); + p_transform = constraint_relative_to_the_skeleton.affine_inverse() * p_transform; + // Origin. + Vector3 orig = skeleton->get_bone_pose(p_id).origin; + Vector3 sub = p_transform.origin - skeleton->get_bone_global_pose(p_id).origin; + t.origin = orig + to_local.xform(sub); + + // Apply transform. + skeleton->set_bone_pose_position(p_id, t.origin); + skeleton->set_bone_pose_rotation(p_id, t.basis.get_rotation_quaternion()); + skeleton->set_bone_pose_scale(p_id, t.basis.get_scale()); + many_bone_ik->update_gizmos(); +} + +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(); + ERR_FAIL_COND(!ne); + + EditorUndoRedoManager *ur = EditorUndoRedoManager::get_singleton(); + ur->create_action(TTR("Set Bone Transform")); + if (ne->get_tool_mode() == Node3DEditor::TOOL_MODE_SELECT || ne->get_tool_mode() == Node3DEditor::TOOL_MODE_ROTATE) { + for (int i = 0; i < p_ids.size(); i++) { + int32_t constraint_i = many_bone_ik->find_constraint(skeleton->get_bone_name(p_ids[i])); + float from_original = many_bone_ik->get_joint_twist(constraint_i).x; + float range = many_bone_ik->get_joint_twist(constraint_i).y; + ur->add_do_method(many_bone_ik, "set_kusudama_twist", constraint_i, Vector2(skeleton->get_bone_pose(p_ids[i]).get_basis().get_euler().y, range)); + ur->add_undo_method(many_bone_ik, "set_kusudama_twist", constraint_i, Vector2(from_original, range)); + ur->add_do_method(many_bone_ik, "set_dirty"); + ur->add_undo_method(many_bone_ik, "set_dirty"); + } + } + ur->commit_action(); +} + +void ManyBoneIK3DGizmoPlugin::_draw_handles() { + if (!many_bone_ik) { + return; + } + Skeleton3D *skeleton = many_bone_ik->get_skeleton(); + ERR_FAIL_COND(!skeleton); + const int bone_count = skeleton->get_bone_count(); + + handles_mesh->clear_surfaces(); + + if (bone_count) { + handles_mesh_instance->show(); + + handles_mesh->surface_begin(Mesh::PRIMITIVE_POINTS); + + for (int i = 0; i < bone_count; i++) { + Color c; + if (i == many_bone_ik->get_ui_selected_bone()) { + c = Color(1, 1, 0); + } else { + c = Color(0.1, 0.25, 0.8); + } + Vector3 point = skeleton->get_bone_global_pose(i).origin; + handles_mesh->surface_set_color(c); + handles_mesh->surface_add_vertex(point); + } + handles_mesh->surface_end(); + handles_mesh->surface_set_material(0, handle_material); + } else { + handles_mesh_instance->hide(); + } +} + +void ManyBoneIK3DGizmoPlugin::_draw_gizmo() { + if (!many_bone_ik) { + return; + } + Skeleton3D *skeleton = many_bone_ik->get_skeleton(); + if (!skeleton) { + return; + } + + // If you call get_bone_global_pose() while drawing the surface, such as toggle rest mode, + // the skeleton update will be done first and + // the drawing surface will be interrupted once and an error will occur. + skeleton->force_update_all_dirty_bones(); + + // Handles. + if (edit_mode) { + _draw_handles(); + } else { + _hide_handles(); + } +} + +void ManyBoneIK3DGizmoPlugin::_update_gizmo_visible() { + if (!many_bone_ik) { + return; + } + Skeleton3D *skeleton = many_bone_ik->get_skeleton(); + if (!skeleton) { + return; + } + _subgizmo_selection_change(); + if (edit_mode) { + int32_t selected_bone = many_bone_ik->get_ui_selected_bone(); + if (selected_bone == -1) { +#ifdef TOOLS_ENABLED + skeleton->set_transform_gizmo_visible(false); +#endif + } else { +#ifdef TOOLS_ENABLED + if (skeleton->is_bone_enabled(selected_bone) && !skeleton->is_show_rest_only()) { + skeleton->set_transform_gizmo_visible(true); + } else { + skeleton->set_transform_gizmo_visible(false); + } +#endif + } + } else { +#ifdef TOOLS_ENABLED + skeleton->set_transform_gizmo_visible(true); +#endif + } + _draw_gizmo(); +} + +void ManyBoneIK3DGizmoPlugin::_subgizmo_selection_change() { + if (!many_bone_ik) { + return; + } + Skeleton3D *skeleton = many_bone_ik->get_skeleton(); + if (!skeleton) { + return; + } + // Once validated by subgizmos_intersect_ray, but required if through inspector's bones tree. + if (!edit_mode) { + skeleton->clear_subgizmo_selection(); + return; + } + int selected = -1; + if (many_bone_ik) { + selected = many_bone_ik->get_ui_selected_bone(); + } + + if (selected >= 0) { + Vector> gizmos = skeleton->get_gizmos(); + for (int i = 0; i < gizmos.size(); i++) { + Ref gizmo = gizmos[i]; + if (!gizmo.is_valid()) { + continue; + } + Ref plugin = gizmo->get_plugin(); + if (!plugin.is_valid()) { + continue; + } + skeleton->set_subgizmo_selection(gizmo, selected, skeleton->get_bone_global_pose(selected)); + break; + } + } else { + skeleton->clear_subgizmo_selection(); + } +} + +void ManyBoneIK3DGizmoPlugin::edit_mode_toggled(const bool pressed) { + edit_mode = pressed; + _update_gizmo_visible(); +} + +void ManyBoneIK3DGizmoPlugin::_hide_handles() { + handles_mesh_instance->hide(); +} + +void ManyBoneIK3DGizmoPlugin::_notifications(int32_t p_what) { + switch (p_what) { + case EditorNode3DGizmoPlugin::NOTIFICATION_POSTINITIALIZE: { + kusudama_shader->set_code(MANY_BONE_IKKUSUDAMA_SHADER); + handle_material = Ref(memnew(ShaderMaterial)); + handle_shader = Ref(memnew(Shader)); + handle_shader->set_code(R"( +// Skeleton 3D gizmo handle shader. + +shader_type spatial; +render_mode unshaded, shadows_disabled, depth_draw_always; +uniform sampler2D texture_albedo : source_color; +uniform float point_size : hint_range(0,128) = 32; +void vertex() { + if (!OUTPUT_IS_SRGB) { + COLOR.rgb = mix( pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb* (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)) ); + } + VERTEX = VERTEX; + POSITION = PROJECTION_MATRIX * VIEW_MATRIX * MODEL_MATRIX * vec4(VERTEX.xyz, 1.0); + POSITION.z = mix(POSITION.z, 0, 0.999); + POINT_SIZE = point_size; +} +void fragment() { + vec4 albedo_tex = texture(texture_albedo,POINT_COORD); + vec3 col = albedo_tex.rgb + COLOR.rgb; + col = vec3(min(col.r,1.0),min(col.g,1.0),min(col.b,1.0)); + ALBEDO = col; + if (albedo_tex.a < 0.5) { discard; } + ALPHA = albedo_tex.a; +} +)"); + handle_material->set_shader(handle_shader); + Ref handle = EditorNode::get_singleton()->get_gui_base()->get_theme_icon(SNAME("EditorBoneHandle"), SNAME("EditorIcons")); + handle_material->set_shader_parameter("point_size", handle->get_width()); + handle_material->set_shader_parameter("texture_albedo", handle); + + handles_mesh_instance = memnew(MeshInstance3D); + handles_mesh_instance->set_cast_shadows_setting(GeometryInstance3D::SHADOW_CASTING_SETTING_OFF); + handles_mesh_instance->set_mesh(handles_mesh); + edit_mode_button = memnew(Button); + edit_mode_button->set_text(TTR("Edit Mode")); + edit_mode_button->set_flat(true); + 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, &ManyBoneIK3DGizmoPlugin::edit_mode_toggled)); + edit_mode = false; + create_material("lines_primary", Color(0.93725490570068, 0.19215686619282, 0.22352941334248), true, true, true); + + unselected_mat = Ref(memnew(StandardMaterial3D)); + unselected_mat->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + unselected_mat->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + unselected_mat->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true); + unselected_mat->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true); + + selected_mat = Ref(memnew(ShaderMaterial)); + selected_sh = Ref(memnew(Shader)); + selected_sh->set_code(R"( +// Skeleton 3D gizmo bones shader. + +shader_type spatial; +render_mode unshaded, shadows_disabled; +void vertex() { + if (!OUTPUT_IS_SRGB) { + COLOR.rgb = mix( pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb* (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)) ); + } + VERTEX = VERTEX; + POSITION = PROJECTION_MATRIX * VIEW_MATRIX * MODEL_MATRIX * vec4(VERTEX.xyz, 1.0); + POSITION.z = mix(POSITION.z, POSITION.w, 0.999); +} +void fragment() { + ALBEDO = COLOR.rgb; + ALPHA = COLOR.a; +} +)"); + selected_mat->set_shader(selected_sh); + + // Register properties in editor settings. + EDITOR_DEF("editors/3d_gizmos/gizmo_colors/skeleton", Color(1, 0.8, 0.4)); + EDITOR_DEF("editors/3d_gizmos/gizmo_colors/selected_bone", Color(0.8, 0.3, 0.0)); + EDITOR_DEF("editors/3d_gizmos/gizmo_settings/bone_axis_length", (float)0.1); + EDITOR_DEF("editors/3d_gizmos/gizmo_settings/bone_shape", 1); + EditorSettings::get_singleton()->add_property_hint(PropertyInfo(Variant::INT, "editors/3d_gizmos/gizmo_settings/bone_shape", PROPERTY_HINT_ENUM, "Wire,Octahedron")); + Node3DEditor::get_singleton()->add_control_to_menu_panel(edit_mode_button); + } break; + case EditorNode3DGizmoPlugin::NOTIFICATION_PREDELETE: { + Node3DEditor::get_singleton()->remove_control_from_menu_panel(edit_mode_button); + } break; + } +} diff --git a/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.h b/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.h new file mode 100644 index 000000000000..99c7e3fd74e3 --- /dev/null +++ b/modules/many_bone_ik/editor/many_bone_ik_3d_gizmo_plugin.h @@ -0,0 +1,101 @@ +/**************************************************************************/ +/* many_bone_ik_3d_gizmo_plugin.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_GIZMO_PLUGIN_H +#define MANY_BONE_IK_3D_GIZMO_PLUGIN_H + +#include "../src/ik_bone_3d.h" +#include "../src/many_bone_ik_3d.h" + +#include "editor/editor_inspector.h" +#include "editor/editor_settings.h" +#include "editor/plugins/skeleton_3d_editor_plugin.h" +#include "scene/3d/camera_3d.h" +#include "scene/3d/mesh_instance_3d.h" +#include "scene/3d/node_3d.h" +#include "scene/3d/skeleton_3d.h" +#include "scene/resources/immediate_mesh.h" +#include "scene/resources/material.h" + +class Joint; +class PhysicalBone3D; +class ManyBoneIKEditorPlugin; +class Button; + +class ManyBoneIK3DGizmoPlugin : public EditorNode3DGizmoPlugin { + GDCLASS(ManyBoneIK3DGizmoPlugin, EditorNode3DGizmoPlugin); + Ref kusudama_shader = memnew(Shader); + + Ref unselected_mat; + Ref selected_mat; + Ref selected_sh = memnew(Shader); + + MeshInstance3D *handles_mesh_instance = nullptr; + Ref handles_mesh = memnew(ImmediateMesh); + Ref handle_material = memnew(ShaderMaterial); + Ref handle_shader; + ManyBoneIK3D *many_bone_ik = nullptr; + Button *edit_mode_button = nullptr; + bool edit_mode = false; + +protected: + static void _bind_methods(); + void _notifications(int32_t p_what); + +public: + const Color bone_color = EditorSettings::get_singleton()->get("editors/3d_gizmos/gizmo_colors/skeleton"); + const int32_t KUSUDAMA_MAX_CONES = 10; + bool has_gizmo(Node3D *p_spatial) override; + 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, 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; + void commit_subgizmos(const EditorNode3DGizmo *p_gizmo, const Vector &p_ids, const Vector &p_restore, bool p_cancel) override; + + void edit_mode_toggled(const bool pressed); + void _subgizmo_selection_change(); + void _update_gizmo_visible(); + void _draw_gizmo(); + + void _draw_handles(); + void _hide_handles(); +}; + +class EditorPluginManyBoneIK : public EditorPlugin { + GDCLASS(EditorPluginManyBoneIK, EditorPlugin); + +public: + EditorPluginManyBoneIK(); +}; + +#endif // MANY_BONE_IK_3D_GIZMO_PLUGIN_H diff --git a/modules/many_bone_ik/editor/many_bone_ik_shader.h b/modules/many_bone_ik/editor/many_bone_ik_shader.h new file mode 100644 index 000000000000..d0b13d335ffc --- /dev/null +++ b/modules/many_bone_ik/editor/many_bone_ik_shader.h @@ -0,0 +1,207 @@ +/**************************************************************************/ +/* many_bone_ik_shader.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_SHADER_H +#define MANY_BONE_IK_SHADER_H + +// Skeleton 3D gizmo kusudama constraint shader. +static constexpr char MANY_BONE_IKKUSUDAMA_SHADER[] = R"( +shader_type spatial; +render_mode depth_draw_always; + +uniform vec4 kusudama_color : source_color = vec4(0.58039218187332, 0.27058824896812, 0.00784313771874, 1.0); +uniform int cone_count = 0; + +// 0,0,0 is the center of the kusudama. The kusudamas have their own bases that automatically get reoriented such that +y points in the direction that is the weighted average of the limitcones on the kusudama. +// But, if you have a kusuduma with just 1 open_cone, then in general that open_cone should be 0,1,0 in the kusudama's basis unless the user has specifically specified otherwise. + +uniform vec4 cone_sequence[30]; + +// This shader can display up to 10 cones (represented by 30 4d vectors) +// Each group of 4 represents the xyz coordinates of the cone direction +// vector in model space and the fourth element represents radius + +// TODO: fire 2022-05-26 +// Use a texture to store bone parameters. +// Use the uv to get the row of the bone. + +varying vec3 normal_model_dir; +varying vec4 vert_model_color; + +bool is_in_inter_cone_path(in vec3 normal_dir, in vec4 tangent_1, in vec4 cone_1, in vec4 tangent_2, in vec4 cone_2) { + vec3 c1xc2 = cross(cone_1.xyz, cone_2.xyz); + float c1c2dir = dot(normal_dir, c1xc2); + + if (c1c2dir < 0.0) { + vec3 c1xt1 = cross(cone_1.xyz, tangent_1.xyz); + vec3 t1xc2 = cross(tangent_1.xyz, cone_2.xyz); + float c1t1dir = dot(normal_dir, c1xt1); + float t1c2dir = dot(normal_dir, t1xc2); + + return (c1t1dir > 0.0 && t1c2dir > 0.0); + + } else { + vec3 t2xc1 = cross(tangent_2.xyz, cone_1.xyz); + vec3 c2xt2 = cross(cone_2.xyz, tangent_2.xyz); + float t2c1dir = dot(normal_dir, t2xc1); + float c2t2dir = dot(normal_dir, c2xt2); + + return (c2t2dir > 0.0 && t2c1dir > 0.0); + } +} + +//determines the current draw condition based on the desired draw condition in the setToArgument +// -3 = disallowed entirely; +// -2 = disallowed and on tangent_cone boundary +// -1 = disallowed and on control_cone boundary +// 0 = allowed and empty; +// 1 = allowed and on control_cone boundary +// 2 = allowed and on tangent_cone boundary +int get_allowability_condition(in int current_condition, in int set_to) { + if((current_condition == -1 || current_condition == -2) + && set_to >= 0) { + return current_condition *= -1; + } else if(current_condition == 0 && (set_to == -1 || set_to == -2)) { + return set_to *=-2; + } + return max(current_condition, set_to); +} + +// returns 1 if normal_dir is beyond (cone.a) radians from the cone.rgb +// returns 0 if normal_dir is within (cone.a + boundary_width) radians from the cone.rgb +// return -1 if normal_dir is less than (cone.a) radians from the cone.rgb +int is_in_cone(in vec3 normal_dir, in vec4 cone, in float boundary_width) { + float arc_dist_to_cone = acos(dot(normal_dir, cone.rgb)); + if (arc_dist_to_cone > (cone.a+(boundary_width/2.))) { + return 1; + } + if (arc_dist_to_cone < (cone.a-(boundary_width/2.))) { + return -1; + } + return 0; +} + +// Returns a color corresponding to the allowability of this region, +// or otherwise the boundaries corresponding +// to various cones and tangent_cone. +vec4 color_allowed(in vec3 normal_dir, in int cone_counts, in float boundary_width) { + int current_condition = -3; + if (cone_counts == 1) { + vec4 cone = cone_sequence[0]; + int in_cone = is_in_cone(normal_dir, cone, boundary_width); + bool is_in_cone = in_cone == 0; + if (is_in_cone) { + in_cone = -1; + } else { + if (in_cone < 0) { + in_cone = 0; + } else { + in_cone = -3; + } + } + current_condition = get_allowability_condition(current_condition, in_cone); + } else { + for(int i=0; i < (cone_counts-1)*3; i=i+3) { + normal_dir = normalize(normal_dir); + + vec4 cone_1 = cone_sequence[i+0]; + vec4 tangent_1 = cone_sequence[i+1]; + vec4 tangent_2 = cone_sequence[i+2]; + vec4 cone_2 = cone_sequence[i+3]; + + int inCone1 = is_in_cone(normal_dir, cone_1, boundary_width); + if (inCone1 == 0) { + inCone1 = -1; + } else { + if (inCone1 < 0) { + inCone1 = 0; + } else { + inCone1 = -3; + } + } + current_condition = get_allowability_condition(current_condition, inCone1); + + int inCone2 = is_in_cone(normal_dir, cone_2, boundary_width); + if (inCone2 == 0) { + inCone2 = -1; + } else { + if (inCone2 < 0) { + inCone2 = 0; + } else { + inCone2 = -3; + } + } + current_condition = get_allowability_condition(current_condition, inCone2); + + int in_tan_1 = is_in_cone(normal_dir, tangent_1, boundary_width); + int in_tan_2 = is_in_cone(normal_dir, tangent_2, boundary_width); + + if (float(in_tan_1) < 1. || float(in_tan_2) < 1.) { + in_tan_1 = in_tan_1 == 0 ? -2 : -3; + current_condition = get_allowability_condition(current_condition, in_tan_1); + in_tan_2 = in_tan_2 == 0 ? -2 : -3; + current_condition = get_allowability_condition(current_condition, in_tan_2); + } else { + bool in_intercone = is_in_inter_cone_path(normal_dir, tangent_1, cone_1, tangent_2, cone_2); + int intercone_condition = in_intercone ? 0 : -3; + current_condition = get_allowability_condition(current_condition, intercone_condition); + } + } + } + vec4 result = vert_model_color; + bool is_disallowed_entirely = current_condition == -3; + bool is_disallowed_on_tangent_cone_boundary = current_condition == -2; + bool is_disallowed_on_control_cone_boundary = current_condition == -1; + if (is_disallowed_entirely || is_disallowed_on_tangent_cone_boundary || is_disallowed_on_control_cone_boundary) { + return result; + } else { + return vec4(0.0, 0.0, 0.0, 0.0); + } + return result; +} + +void vertex() { + normal_model_dir = CUSTOM0.rgb; + vert_model_color.rgb = kusudama_color.rgb; + // Draw the spheres in front of the background. + VERTEX = VERTEX; + POSITION = PROJECTION_MATRIX * VIEW_MATRIX * MODEL_MATRIX * vec4(VERTEX.xyz, 1.0); + POSITION.z = mix(POSITION.z, POSITION.w, 0.999); +} + +void fragment() { + vec4 result_color_allowed = vec4(0.0, 0.0, 0.0, 0.0); + result_color_allowed = color_allowed(normal_model_dir, cone_count, 0.02); + ALBEDO = result_color_allowed.rgb; + ALPHA = 0.8; +} +)"; + +#endif // MANY_BONE_IK_SHADER_H diff --git a/modules/many_bone_ik/register_types.cpp b/modules/many_bone_ik/register_types.cpp new file mode 100644 index 000000000000..5b7ed4fd15c0 --- /dev/null +++ b/modules/many_bone_ik/register_types.cpp @@ -0,0 +1,68 @@ +/**************************************************************************/ +/* 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_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); + } +} + +void uninitialize_many_bone_ik_module(ModuleInitializationLevel p_level) { + if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) { + return; + } +} diff --git a/modules/many_bone_ik/register_types.h b/modules/many_bone_ik/register_types.h new file mode 100644 index 000000000000..7bb117a1b9a3 --- /dev/null +++ b/modules/many_bone_ik/register_types.h @@ -0,0 +1,39 @@ +/**************************************************************************/ +/* register_types.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef MANY_BONE_IK_REGISTER_TYPES_H +#define MANY_BONE_IK_REGISTER_TYPES_H + +#include "modules/register_module_types.h" + +void initialize_many_bone_ik_module(ModuleInitializationLevel p_level); +void uninitialize_many_bone_ik_module(ModuleInitializationLevel p_level); + +#endif // MANY_BONE_IK_REGISTER_TYPES_H diff --git a/modules/many_bone_ik/src/ik_bone_3d.cpp b/modules/many_bone_ik/src/ik_bone_3d.cpp new file mode 100644 index 000000000000..473475887ea1 --- /dev/null +++ b/modules/many_bone_ik/src/ik_bone_3d.cpp @@ -0,0 +1,364 @@ +/**************************************************************************/ +/* ik_bone_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "ik_bone_3d.h" +#include "ik_kusudama_3d.h" +#include "many_bone_ik_3d.h" +#include "math/ik_node_3d.h" +#include + +void IKBone3D::set_bone_id(BoneId p_bone_id, Skeleton3D *p_skeleton) { + ERR_FAIL_NULL(p_skeleton); + bone_id = p_bone_id; +} + +BoneId IKBone3D::get_bone_id() const { + return bone_id; +} + +void IKBone3D::set_parent(const Ref &p_parent) { + ERR_FAIL_NULL(p_parent); + parent = p_parent; + if (parent.is_valid()) { + parent->children.push_back(this); + godot_skeleton_aligned_transform->set_parent(parent->godot_skeleton_aligned_transform); + constraint_orientation_transform->set_parent(parent->godot_skeleton_aligned_transform); + constraint_twist_transform->set_parent(parent->godot_skeleton_aligned_transform); + } +} + +void IKBone3D::update_default_bone_direction_transform(Skeleton3D *p_skeleton) { + Vector3 child_centroid; + int child_count = 0; + + for (Ref &ik_bone : children) { + child_centroid += ik_bone->get_ik_transform()->get_global_transform().origin; + child_count++; + } + + if (child_count > 0) { + child_centroid /= child_count; + } else { + const PackedInt32Array &bone_children = p_skeleton->get_bone_children(bone_id); + for (BoneId child_bone_idx : bone_children) { + child_centroid += p_skeleton->get_bone_global_pose(child_bone_idx).origin; + } + child_centroid /= bone_children.size(); + } + + const Vector3 &godot_bone_origin = godot_skeleton_aligned_transform->get_global_transform().origin; + child_centroid -= godot_bone_origin; + + if (Math::is_zero_approx(child_centroid.length_squared())) { + if (parent.is_valid()) { + child_centroid = parent->get_bone_direction_transform()->get_global_transform().basis.get_column(Vector3::AXIS_Y); + } else { + child_centroid = get_bone_direction_transform()->get_global_transform().basis.get_column(Vector3::AXIS_Y); + } + } + + if (!Math::is_zero_approx(child_centroid.length_squared()) && (children.size() || p_skeleton->get_bone_children(bone_id).size())) { + child_centroid.normalize(); + Vector3 bone_direction = bone_direction_transform->get_global_transform().basis.get_column(Vector3::AXIS_Y); + bone_direction.normalize(); + bone_direction_transform->rotate_local_with_global(Quaternion(child_centroid, bone_direction)); + } +} + +void IKBone3D::update_default_constraint_transform() { + Ref parent_bone = get_parent(); + if (parent_bone.is_valid()) { + Transform3D parent_bone_aligned_transform = 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 IKBone3D::get_parent() const { + return parent; +} + +void IKBone3D::set_pin(const Ref &p_pin) { + ERR_FAIL_NULL(p_pin); + pin = p_pin; +} + +Ref IKBone3D::get_pin() const { + return pin; +} + +void IKBone3D::set_pose(const Transform3D &p_transform) { + godot_skeleton_aligned_transform->set_transform(p_transform); +} + +Transform3D IKBone3D::get_pose() const { + return godot_skeleton_aligned_transform->get_transform(); +} + +void IKBone3D::set_global_pose(const Transform3D &p_transform) { + godot_skeleton_aligned_transform->set_global_transform(p_transform); + Transform3D transform = constraint_orientation_transform->get_transform(); + transform.origin = godot_skeleton_aligned_transform->get_transform().origin; + constraint_orientation_transform->set_transform(transform); + constraint_orientation_transform->_propagate_transform_changed(); +} + +Transform3D IKBone3D::get_global_pose() const { + return godot_skeleton_aligned_transform->get_global_transform(); +} + +Transform3D IKBone3D::get_bone_direction_global_pose() const { + return bone_direction_transform->get_global_transform(); +} + +void IKBone3D::set_initial_pose(Skeleton3D *p_skeleton) { + ERR_FAIL_NULL(p_skeleton); + if (bone_id == -1) { + return; + } + Transform3D bone_origin_to_parent_origin = p_skeleton->get_bone_pose(bone_id); + set_pose(bone_origin_to_parent_origin); +} + +void IKBone3D::set_skeleton_bone_pose(Skeleton3D *p_skeleton) { + ERR_FAIL_NULL(p_skeleton); + Transform3D bone_to_parent = get_pose(); + p_skeleton->set_bone_pose_position(bone_id, bone_to_parent.origin); + if (!bone_to_parent.basis.is_finite()) { + bone_to_parent.basis = Basis(); + } + p_skeleton->set_bone_pose_rotation(bone_id, bone_to_parent.basis.get_rotation_quaternion()); + p_skeleton->set_bone_pose_scale(bone_id, bone_to_parent.basis.get_scale()); +} + +void IKBone3D::create_pin() { + pin = Ref(memnew(IKEffector3D(this))); +} + +bool IKBone3D::is_pinned() const { + return pin.is_valid(); +} + +void IKBone3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_pin"), &IKBone3D::get_pin); + ClassDB::bind_method(D_METHOD("set_pin", "pin"), &IKBone3D::set_pin); + ClassDB::bind_method(D_METHOD("is_pinned"), &IKBone3D::is_pinned); + ClassDB::bind_method(D_METHOD("get_constraint"), &IKBone3D::get_constraint); + ClassDB::bind_method(D_METHOD("get_constraint_orientation_transform"), &IKBone3D::get_constraint_orientation_transform); + ClassDB::bind_method(D_METHOD("get_constraint_twist_transform"), &IKBone3D::get_constraint_twist_transform); +} + +IKBone3D::IKBone3D(StringName p_bone, Skeleton3D *p_skeleton, const Ref &p_parent, Vector> &p_pins, float p_default_dampening, + ManyBoneIK3D *p_many_bone_ik) { + ERR_FAIL_NULL(p_skeleton); + + default_dampening = p_default_dampening; + cos_half_dampen = cos(default_dampening / real_t(2.0)); + set_name(p_bone); + bone_id = p_skeleton->find_bone(p_bone); + if (p_parent.is_valid()) { + set_parent(p_parent); + } + for (Ref elem : p_pins) { + if (elem.is_null()) { + continue; + } + if (elem->get_name() == p_bone) { + create_pin(); + Ref effector = get_pin(); + effector->set_target_node(p_skeleton, elem->get_target_node()); + 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 IKBone3D::get_cos_half_dampen() const { + return cos_half_dampen; +} + +void IKBone3D::set_cos_half_dampen(float p_cos_half_dampen) { + cos_half_dampen = p_cos_half_dampen; +} + +Ref IKBone3D::get_constraint() const { + return constraint; +} + +void IKBone3D::add_constraint(Ref p_constraint) { + constraint = p_constraint; +} + +Ref IKBone3D::get_ik_transform() { + return godot_skeleton_aligned_transform; +} + +Ref IKBone3D::get_constraint_orientation_transform() { + return constraint_orientation_transform; +} + +Ref IKBone3D::get_constraint_twist_transform() { + return constraint_twist_transform; +} + +void IKBone3D::set_constraint_orientation_transform(Ref p_transform) { + constraint_orientation_transform = p_transform; +} + +void IKBone3D::set_bone_direction_transform(Ref p_bone_direction) { + bone_direction_transform = p_bone_direction; +} + +Ref IKBone3D::get_bone_direction_transform() { + return bone_direction_transform; +} + +bool IKBone3D::is_orientationally_constrained() { + if (get_constraint().is_null()) { + return false; + } + return get_constraint()->is_orientationally_constrained(); +} + +bool IKBone3D::is_axially_constrained() { + if (get_constraint().is_null()) { + return false; + } + return get_constraint()->is_axially_constrained(); +} + +Vector &IKBone3D::get_cos_half_returnfullness_dampened() { + return cos_half_returnfulness_dampened; +} + +void IKBone3D::set_cos_half_returnfullness_dampened(const Vector &p_value) { + cos_half_returnfulness_dampened = p_value; +} + +Vector &IKBone3D::get_half_returnfullness_dampened() { + return half_returnfulness_dampened; +} + +void IKBone3D::set_half_returnfullness_dampened(const Vector &p_value) { + half_returnfulness_dampened = p_value; +} + +void IKBone3D::set_stiffness(double p_stiffness) { + stiffness = p_stiffness; +} + +double IKBone3D::get_stiffness() const { + return stiffness; +} + +Transform3D IKBone3D::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 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 new file mode 100644 index 000000000000..17b6062f6771 --- /dev/null +++ b/modules/many_bone_ik/src/ik_bone_3d.h @@ -0,0 +1,122 @@ +/**************************************************************************/ +/* ik_bone_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef IK_BONE_3D_H +#define IK_BONE_3D_H + +#include "ik_effector_template_3d.h" +#include "ik_kusudama_3d.h" +#include "ik_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 IKEffector3D; +class ManyBoneIK3D; +class IKBone3D; + +class IKBone3D : public Resource { + GDCLASS(IKBone3D, Resource); + + BoneId bone_id = -1; + Ref parent; + Vector> children; + Ref pin; + + float default_dampening = Math_PI; + float dampening = get_parent().is_null() ? Math_PI : default_dampening; + float cos_half_dampen = Math::cos(dampening / 2.0f); + double cos_half_return_damp = 0.0f; + double return_damp = 0.0f; + Vector cos_half_returnfulness_dampened; + Vector half_returnfulness_dampened; + double stiffness = 0.0; + Ref constraint; + // In the space of the local parent bone transform. + // The origin is the origin of the bone direction transform + // Can be independent and should be calculated + // to keep -y to be the opposite of its bone forward orientation + // To avoid singularity that is ambiguous. + Ref constraint_orientation_transform = Ref(memnew(IKNode3D())); + Ref constraint_twist_transform = Ref(memnew(IKNode3D())); + Ref godot_skeleton_aligned_transform = Ref(memnew(IKNode3D())); // The bone's actual transform. + Ref bone_direction_transform = Ref(memnew(IKNode3D())); // Physical direction of the bone. Calculate Y is the bone up. + +protected: + static void _bind_methods(); + +public: + Vector &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(); + IKBone3D() {} + IKBone3D(StringName p_bone, Skeleton3D *p_skeleton, const Ref &p_parent, Vector> &p_pins, float p_default_dampening = Math_PI, ManyBoneIK3D *p_many_bone_ik = nullptr); + ~IKBone3D() {} + float get_cos_half_dampen() const; + void set_cos_half_dampen(float p_cos_half_dampen); + 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 new file mode 100644 index 000000000000..c0968f616b0c --- /dev/null +++ b/modules/many_bone_ik/src/ik_bone_segment_3d.cpp @@ -0,0 +1,427 @@ +/**************************************************************************/ +/* 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 IKBoneSegment3D::get_root() const { + return root; +} + +Ref IKBoneSegment3D::get_tip() const { + return tip; +} + +bool IKBoneSegment3D::is_pinned() const { + ERR_FAIL_NULL_V(tip, false); + return tip->is_pinned(); +} + +Vector> IKBoneSegment3D::get_child_segments() const { + return child_segments; +} + +void IKBoneSegment3D::create_bone_list(Vector> &p_list, bool p_recursive) 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 IKBoneSegment3D::update_pinned_list(Vector> &r_weights) { + for (int32_t chain_i = 0; chain_i < child_segments.size(); chain_i++) { + Ref chain = child_segments[chain_i]; + chain->update_pinned_list(r_weights); + } + if (is_pinned()) { + effector_list.push_back(tip->get_pin()); + } + double motion_propagation_factor = is_pinned() ? tip->get_pin()->motion_propagation_factor : 1.0; + if (motion_propagation_factor > 0.0) { + for (Ref child : child_segments) { + effector_list.append_array(child->effector_list); + } + } +} + +void IKBoneSegment3D::_update_optimal_rotation(Ref p_for_bone, double p_damp, bool p_translate, bool p_constraint_mode, int32_t current_iteration, int32_t total_iterations) { + ERR_FAIL_NULL(p_for_bone); + _update_target_headings(p_for_bone, &heading_weights, &target_headings); + _update_tip_headings(p_for_bone, &tip_headings); + _set_optimal_rotation(p_for_bone, &tip_headings, &target_headings, &heading_weights, p_damp, p_translate, p_constraint_mode); +} + +Quaternion IKBoneSegment3D::clamp_to_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 IKBoneSegment3D::_get_manual_msd(const PackedVector3Array &r_htip, const PackedVector3Array &r_htarget, const Vector &p_weights) { + float manual_RMSD = 0.0f; + float w_sum = 0.0f; + for (int i = 0; i < r_htarget.size(); i++) { + float x_d = r_htarget[i].x - r_htip[i].x; + float y_d = r_htarget[i].y - r_htip[i].y; + float z_d = r_htarget[i].z - r_htip[i].z; + float mag_sq = p_weights[i] * (x_d * x_d + y_d * y_d + z_d * z_d); + manual_RMSD += mag_sq; + w_sum += p_weights[i]; + } + manual_RMSD /= w_sum * w_sum; + return manual_RMSD; +} + +void IKBoneSegment3D::_set_optimal_rotation(Ref p_for_bone, PackedVector3Array *r_htip, PackedVector3Array *r_htarget, Vector *r_weights, float p_dampening, bool p_translate, bool p_constraint_mode, double current_iteration, double total_iterations) { + ERR_FAIL_NULL(p_for_bone); + ERR_FAIL_NULL(r_htip); + ERR_FAIL_NULL(r_htarget); + ERR_FAIL_NULL(r_weights); + + _update_target_headings(p_for_bone, &heading_weights, &target_headings); + Transform3D prev_transform = p_for_bone->get_pose(); + bool got_closer = true; + double bone_damp = p_for_bone->get_cos_half_dampen(); + int i = 0; + do { + _update_tip_headings(p_for_bone, &tip_headings); + if (!p_constraint_mode) { + QCP qcp = QCP(evec_prec); + Basis rotation = qcp.weighted_superpose(*r_htip, *r_htarget, *r_weights, p_translate); + Vector3 translation = qcp.get_translation(); + double dampening = (p_dampening != -1.0) ? p_dampening : bone_damp; + rotation = clamp_to_cos_half_angle(rotation.get_rotation_quaternion(), 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 IKBoneSegment3D::_update_target_headings(Ref p_for_bone, Vector *r_weights, PackedVector3Array *r_target_headings) { + ERR_FAIL_NULL(p_for_bone); + ERR_FAIL_NULL(r_weights); + ERR_FAIL_NULL(r_target_headings); + int32_t last_index = 0; + for (int32_t effector_i = 0; effector_i < effector_list.size(); effector_i++) { + Ref effector = effector_list[effector_i]; + if (effector.is_null()) { + continue; + } + last_index = effector->update_effector_target_headings(r_target_headings, last_index, p_for_bone, &heading_weights); + } +} + +void IKBoneSegment3D::_update_tip_headings(Ref p_for_bone, PackedVector3Array *r_heading_tip) { + ERR_FAIL_NULL(r_heading_tip); + ERR_FAIL_NULL(p_for_bone); + int32_t last_index = 0; + for (int32_t effector_i = 0; effector_i < effector_list.size(); effector_i++) { + Ref effector = effector_list[effector_i]; + if (effector.is_null()) { + continue; + } + last_index = effector->update_effector_tip_headings(r_heading_tip, last_index, p_for_bone); + } +} + +void IKBoneSegment3D::segment_solver(const Vector &p_damp, float p_default_damp, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iteration) { + for (Ref child : child_segments) { + if (child.is_null()) { + continue; + } + child->segment_solver(p_damp, p_default_damp, p_constraint_mode, p_current_iteration, p_total_iteration); + } + bool is_translate = parent_segment.is_null(); + if (is_translate) { + Vector damp = p_damp; + damp.fill(Math_PI); + _qcp_solver(damp, Math_PI, is_translate, p_constraint_mode, p_current_iteration, p_total_iteration); + return; + } + _qcp_solver(p_damp, p_default_damp, is_translate, p_constraint_mode, p_current_iteration, p_total_iteration); +} + +void IKBoneSegment3D::_qcp_solver(const Vector &p_damp, float p_default_damp, bool p_translate, bool p_constraint_mode, int32_t p_current_iteration, int32_t p_total_iterations) { + for (Ref current_bone : bones) { + float damp = p_default_damp; + bool is_valid_access = !(unlikely((p_damp.size()) < 0 || (current_bone->get_bone_id()) >= (p_damp.size()))); + if (is_valid_access) { + damp = p_damp[current_bone->get_bone_id()]; + } + bool is_non_default_damp = p_default_damp < damp; + if (is_non_default_damp) { + damp = p_default_damp; + } + _update_optimal_rotation(current_bone, damp, p_translate, p_constraint_mode, p_current_iteration, p_total_iterations); + } +} + +void IKBoneSegment3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("is_pinned"), &IKBoneSegment3D::is_pinned); + ClassDB::bind_method(D_METHOD("get_ik_bone", "bone"), &IKBoneSegment3D::get_ik_bone); +} + +IKBoneSegment3D::IKBoneSegment3D(Skeleton3D *p_skeleton, StringName p_root_bone_name, Vector> &p_pins, ManyBoneIK3D *p_many_bone_ik, const Ref &p_parent, + BoneId p_root, BoneId p_tip, int32_t p_stabilizing_pass_count) { + root = p_root; + tip = p_tip; + skeleton = p_skeleton; + root = Ref(memnew(IKBone3D(p_root_bone_name, p_skeleton, p_parent, p_pins, Math_PI, p_many_bone_ik))); + if (p_parent.is_valid()) { + root_segment = p_parent->root_segment; + } else { + root_segment = Ref(this); + } + root_segment->bone_map[root->get_bone_id()] = root; + if (p_parent.is_valid()) { + parent_segment = p_parent; + root->set_parent(p_parent->get_tip()); + } + default_stabilizing_pass_count = p_stabilizing_pass_count; +} + +void IKBoneSegment3D::_enable_pinned_descendants() { + pinned_descendants = true; +} + +bool IKBoneSegment3D::_has_pinned_descendants() { + return pinned_descendants; +} + +Ref IKBoneSegment3D::get_ik_bone(BoneId p_bone) const { + if (!bone_map.has(p_bone)) { + return Ref(); + } + return bone_map[p_bone]; +} + +void IKBoneSegment3D::create_headings_arrays() { + Vector> penalty_array; + Vector> new_pinned_bones; + recursive_create_penalty_array(this, penalty_array, new_pinned_bones, 1.0); + pinned_bones.resize(new_pinned_bones.size()); + int32_t total_headings = 0; + for (const Vector ¤t_penalty_array : penalty_array) { + total_headings += current_penalty_array.size(); + } + for (int32_t bone_i = 0; bone_i < new_pinned_bones.size(); bone_i++) { + pinned_bones.write[bone_i] = new_pinned_bones[bone_i]; + } + target_headings.resize(total_headings); + tip_headings.resize(total_headings); + tip_headings_uniform.resize(total_headings); + heading_weights.resize(total_headings); + int currentHeading = 0; + for (const Vector ¤t_penalty_array : penalty_array) { + for (double ad : current_penalty_array) { + heading_weights.write[currentHeading] = ad; + target_headings.write[currentHeading] = Vector3(); + tip_headings.write[currentHeading] = Vector3(); + tip_headings_uniform.write[currentHeading] = Vector3(); + currentHeading++; + } + } +} + +void IKBoneSegment3D::recursive_create_penalty_array(Ref p_bone_segment, Vector> &r_penalty_array, Vector> &r_pinned_bones, double p_falloff) { + if (p_falloff <= 0.0) { + return; + } + + double current_falloff = 1.0; + + if (p_bone_segment->is_pinned()) { + Ref current_tip = p_bone_segment->get_tip(); + Ref pin = current_tip->get_pin(); + double weight = pin->get_weight(); + Vector inner_weight_array; + inner_weight_array.push_back(weight * p_falloff); + + double max_pin_weight = MAX(MAX(pin->get_direction_priorities().x, pin->get_direction_priorities().y), pin->get_direction_priorities().z); + max_pin_weight = max_pin_weight == 0.0 ? 1.0 : max_pin_weight; + + for (int i = 0; i < 3; ++i) { + double priority = pin->get_direction_priorities()[i]; + if (priority > 0.0) { + double sub_target_weight = weight * (priority / max_pin_weight) * p_falloff; + inner_weight_array.push_back(sub_target_weight); + inner_weight_array.push_back(sub_target_weight); + } + } + + r_penalty_array.push_back(inner_weight_array); + r_pinned_bones.push_back(current_tip); + current_falloff = pin->get_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 IKBoneSegment3D::recursive_create_headings_arrays_for(Ref p_bone_segment) { + p_bone_segment->create_headings_arrays(); + for (Ref segments : p_bone_segment->get_child_segments()) { + recursive_create_headings_arrays_for(segments); + } +} + +void IKBoneSegment3D::generate_default_segments(Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik) { + Ref current_tip = root; + 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 IKBoneSegment3D::_is_parent_of_tip(Ref p_current_tip, BoneId p_tip_bone) { + return skeleton->get_bone_parent(p_current_tip->get_bone_id()) >= p_tip_bone && p_tip_bone != -1; +} + +bool IKBoneSegment3D::_has_multiple_children_or_pinned(Vector &r_children, Ref p_current_tip) { + return r_children.size() > 1 || p_current_tip->is_pinned(); +} + +void IKBoneSegment3D::_process_children(Vector &r_children, Ref p_current_tip, Vector> &r_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik) { + tip = p_current_tip; + Ref parent(this); + + for (int32_t child_i = 0; child_i < r_children.size(); child_i++) { + BoneId child_bone = r_children[child_i]; + String child_name = skeleton->get_bone_name(child_bone); + Ref child_segment = _create_child_segment(child_name, r_pins, p_root_bone, p_tip_bone, p_many_bone_ik, parent); + + child_segment->generate_default_segments(r_pins, p_root_bone, p_tip_bone, p_many_bone_ik); + + if (child_segment->_has_pinned_descendants()) { + _enable_pinned_descendants(); + child_segments.push_back(child_segment); + } + } +} + +Ref IKBoneSegment3D::_create_child_segment(String &p_child_name, Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik, Ref &p_parent) { + return Ref(memnew(IKBoneSegment3D(skeleton, p_child_name, p_pins, p_many_bone_ik, p_parent, p_root_bone, p_tip_bone))); +} + +Ref IKBoneSegment3D::_create_next_bone(BoneId p_bone_id, Ref p_current_tip, Vector> &p_pins, ManyBoneIK3D *p_many_bone_ik) { + String bone_name = skeleton->get_bone_name(p_bone_id); + Ref next_bone = Ref(memnew(IKBone3D(bone_name, skeleton, p_current_tip, p_pins, p_many_bone_ik->get_default_damp(), p_many_bone_ik))); + root_segment->bone_map[p_bone_id] = next_bone; + + return next_bone; +} + +void IKBoneSegment3D::_finalize_segment(Ref p_current_tip) { + tip = p_current_tip; + + if (tip->is_pinned()) { + _enable_pinned_descendants(); + } + + 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/many_bone_ik/src/ik_bone_segment_3d.h b/modules/many_bone_ik/src/ik_bone_segment_3d.h new file mode 100644 index 000000000000..e5d33c54ce2c --- /dev/null +++ b/modules/many_bone_ik/src/ik_bone_segment_3d.h @@ -0,0 +1,105 @@ +/**************************************************************************/ +/* ik_bone_segment_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef IK_BONE_SEGMENT_3D_H +#define IK_BONE_SEGMENT_3D_H + +#include "ik_bone_3d.h" +#include "ik_effector_3d.h" +#include "ik_effector_template_3d.h" +#include "math/qcp.h" +#include "scene/3d/skeleton_3d.h" + +#include "core/io/resource.h" +#include "core/object/ref_counted.h" + +class IKEffector3D; +class IKBone3D; +class IKLimitCone3D; + +class IKBoneSegment3D : public Resource { + GDCLASS(IKBoneSegment3D, Resource); + Ref root; + Ref tip; + Vector> bones; + Vector> pinned_bones; + Vector> child_segments; // Contains only direct child chains that end with effectors or have child that end with effectors + Ref parent_segment; + Ref root_segment; + Vector> effector_list; + PackedVector3Array target_headings; + PackedVector3Array tip_headings; + PackedVector3Array tip_headings_uniform; + Vector heading_weights; + 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, ManyBoneIK3D *p_many_bone_ik); + Ref _create_child_segment(String &p_child_name, Vector> &p_pins, BoneId p_root_bone, BoneId p_tip_bone, ManyBoneIK3D *p_many_bone_ik, Ref &p_parent); + Ref _create_next_bone(BoneId p_bone_id, Ref p_current_tip, Vector> &p_pins, ManyBoneIK3D *p_many_bone_ik); + void _finalize_segment(Ref p_current_tip); + +protected: + static void _bind_methods(); + +public: + const double evec_prec = static_cast(1E-6); + 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, ManyBoneIK3D *p_many_bone_ik); + IKBoneSegment3D() {} + IKBoneSegment3D(Skeleton3D *p_skeleton, StringName p_root_bone_name, Vector> &p_pins, ManyBoneIK3D *p_many_bone_ik, const Ref &p_parent = nullptr, + BoneId root = -1, BoneId tip = -1, int32_t p_stabilizing_pass_count = 0); + ~IKBoneSegment3D() {} +}; + +#endif // IK_BONE_SEGMENT_3D_H diff --git a/modules/many_bone_ik/src/ik_effector_3d.cpp b/modules/many_bone_ik/src/ik_effector_3d.cpp new file mode 100644 index 000000000000..0a225884f897 --- /dev/null +++ b/modules/many_bone_ik/src/ik_effector_3d.cpp @@ -0,0 +1,183 @@ +/**************************************************************************/ +/* ik_effector_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "ik_effector_3d.h" + +#include "core/typedefs.h" +#include "ik_bone_3d.h" +#include "many_bone_ik_3d.h" +#include "math/ik_node_3d.h" +#include "scene/3d/node_3d.h" + +#ifdef TOOLS_ENABLED +#include "editor/editor_data.h" +#include "editor/editor_node.h" +#endif + +void IKEffector3D::set_target_node(Skeleton3D *p_skeleton, const NodePath &p_target_node_path) { + ERR_FAIL_NULL(p_skeleton); + target_node_path = p_target_node_path; +} + +NodePath IKEffector3D::get_target_node() const { + return target_node_path; +} + +void IKEffector3D::set_target_node_rotation(bool p_use) { + use_target_node_rotation = p_use; +} + +bool IKEffector3D::get_target_node_rotation() const { + return use_target_node_rotation; +} + +Ref IKEffector3D::get_ik_bone_3d() const { + return for_bone; +} + +bool IKEffector3D::is_following_translation_only() const { + return Math::is_zero_approx(direction_priorities.length_squared()); +} + +void IKEffector3D::set_direction_priorities(Vector3 p_direction_priorities) { + direction_priorities = p_direction_priorities; +} + +Vector3 IKEffector3D::get_direction_priorities() const { + return direction_priorities; +} + +void IKEffector3D::update_target_global_transform(Skeleton3D *p_skeleton, ManyBoneIK3D *p_many_bone_ik) { + ERR_FAIL_NULL(p_skeleton); + ERR_FAIL_NULL(for_bone); + Node3D *current_target_node = cast_to(p_many_bone_ik->get_node_or_null(target_node_path)); + if (current_target_node && current_target_node->is_visible_in_tree()) { + target_relative_to_skeleton_origin = p_skeleton->get_global_transform().affine_inverse() * current_target_node->get_global_transform(); + } +} + +Transform3D IKEffector3D::get_target_global_transform() const { + return target_relative_to_skeleton_origin; +} + +int32_t IKEffector3D::update_effector_target_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone, const Vector *p_weights) const { + ERR_FAIL_COND_V(p_index == -1, -1); + ERR_FAIL_NULL_V(p_headings, -1); + ERR_FAIL_NULL_V(p_for_bone, -1); + ERR_FAIL_NULL_V(p_weights, -1); + + int32_t index = p_index; + Vector3 bone_origin_relative_to_skeleton_origin = for_bone->get_bone_direction_global_pose().origin; + p_headings->write[index] = target_relative_to_skeleton_origin.origin - bone_origin_relative_to_skeleton_origin; + index++; + Vector3 priority = get_direction_priorities(); + for (int axis = Vector3::AXIS_X; axis <= Vector3::AXIS_Z; ++axis) { + if (priority[axis] > 0.0) { + real_t w = p_weights->get(index); + Vector3 column = target_relative_to_skeleton_origin.basis.get_column(axis); + + p_headings->write[index] = (column + target_relative_to_skeleton_origin.origin) - bone_origin_relative_to_skeleton_origin; + p_headings->write[index] *= Vector3(w, w, w); + index++; + p_headings->write[index] = (target_relative_to_skeleton_origin.origin - column) - bone_origin_relative_to_skeleton_origin; + p_headings->write[index] *= Vector3(w, w, w); + index++; + } + } + + return index; +} + +int32_t IKEffector3D::update_effector_tip_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone) const { + ERR_FAIL_COND_V(p_index == -1, -1); + ERR_FAIL_NULL_V(p_headings, -1); + ERR_FAIL_NULL_V(p_for_bone, -1); + + Transform3D tip_xform_relative_to_skeleton_origin = for_bone->get_bone_direction_global_pose(); + Basis tip_basis = tip_xform_relative_to_skeleton_origin.basis; + Vector3 bone_origin_relative_to_skeleton_origin = p_for_bone->get_bone_direction_global_pose().origin; + + int32_t index = p_index; + p_headings->write[index] = tip_xform_relative_to_skeleton_origin.origin - bone_origin_relative_to_skeleton_origin; + index++; + double distance = target_relative_to_skeleton_origin.origin.distance_to(bone_origin_relative_to_skeleton_origin); + double scale_by = 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 IKEffector3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_target_node", "skeleton", "node"), + &IKEffector3D::set_target_node); + ClassDB::bind_method(D_METHOD("get_target_node"), + &IKEffector3D::get_target_node); + ClassDB::bind_method(D_METHOD("set_motion_propagation_factor", "amount"), + &IKEffector3D::set_motion_propagation_factor); + ClassDB::bind_method(D_METHOD("get_motion_propagation_factor"), + &IKEffector3D::get_motion_propagation_factor); + + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "motion_propagation_factor"), "set_motion_propagation_factor", "get_motion_propagation_factor"); +} + +void IKEffector3D::set_weight(real_t p_weight) { + weight = p_weight; +} + +real_t IKEffector3D::get_weight() const { + return weight; +} + +IKEffector3D::IKEffector3D(const Ref &p_current_bone) { + ERR_FAIL_NULL(p_current_bone); + for_bone = p_current_bone; +} + +void IKEffector3D::set_motion_propagation_factor(float p_motion_propagation_factor) { + motion_propagation_factor = CLAMP(p_motion_propagation_factor, 0.0, 1.0); +} + +float IKEffector3D::get_motion_propagation_factor() const { + return motion_propagation_factor; +} diff --git a/modules/many_bone_ik/src/ik_effector_3d.h b/modules/many_bone_ik/src/ik_effector_3d.h new file mode 100644 index 000000000000..c0c56e359244 --- /dev/null +++ b/modules/many_bone_ik/src/ik_effector_3d.h @@ -0,0 +1,92 @@ +/**************************************************************************/ +/* ik_effector_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef IK_EFFECTOR_3D_H +#define IK_EFFECTOR_3D_H + +#include "math/ik_node_3d.h" + +#include "core/object/ref_counted.h" +#include "scene/3d/skeleton_3d.h" + +#define MIN_SCALE 0.1 + +class ManyBoneIK3D; +class IKBone3D; + +class IKEffector3D : public Resource { + GDCLASS(IKEffector3D, Resource); + friend class IKBone3D; + friend class IKBoneSegment3D; + + Ref for_bone; + bool use_target_node_rotation = true; + NodePath target_node_path; + ObjectID target_node_cache; + Node *target_node_reference = nullptr; + 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: + IKEffector3D() = default; + void set_weight(real_t p_weight); + real_t get_weight() const; + void set_direction_priorities(Vector3 p_direction_priorities); + Vector3 get_direction_priorities() const; + void update_target_global_transform(Skeleton3D *p_skeleton, ManyBoneIK3D *p_modification = nullptr); + const float MAX_KUSUDAMA_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); + NodePath get_target_node() const; + Transform3D get_target_global_transform() const; + void set_target_node_rotation(bool p_use); + bool get_target_node_rotation() const; + Ref get_ik_bone_3d() const; + bool is_following_translation_only() const; + int32_t update_effector_target_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone, const Vector *p_weights) const; + int32_t update_effector_tip_headings(PackedVector3Array *p_headings, int32_t p_index, Ref p_for_bone) const; + IKEffector3D(const Ref &p_current_bone); +}; + +#endif // IK_EFFECTOR_3D_H diff --git a/modules/many_bone_ik/src/ik_effector_template_3d.cpp b/modules/many_bone_ik/src/ik_effector_template_3d.cpp new file mode 100644 index 000000000000..d9d8d5963dee --- /dev/null +++ b/modules/many_bone_ik/src/ik_effector_template_3d.cpp @@ -0,0 +1,83 @@ +/**************************************************************************/ +/* ik_effector_template_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "ik_effector_template_3d.h" + +#include "many_bone_ik_3d.h" + +void IKEffectorTemplate3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_root_bone"), &IKEffectorTemplate3D::get_root_bone); + ClassDB::bind_method(D_METHOD("set_root_bone", "target_node"), &IKEffectorTemplate3D::set_root_bone); + + ClassDB::bind_method(D_METHOD("get_target_node"), &IKEffectorTemplate3D::get_target_node); + ClassDB::bind_method(D_METHOD("set_target_node", "target_node"), &IKEffectorTemplate3D::set_target_node); + + ClassDB::bind_method(D_METHOD("get_motion_propagation_factor"), &IKEffectorTemplate3D::get_motion_propagation_factor); + ClassDB::bind_method(D_METHOD("set_motion_propagation_factor", "motion_propagation_factor"), &IKEffectorTemplate3D::set_motion_propagation_factor); + + ClassDB::bind_method(D_METHOD("get_weight"), &IKEffectorTemplate3D::get_weight); + ClassDB::bind_method(D_METHOD("set_weight", "weight"), &IKEffectorTemplate3D::set_weight); + + ClassDB::bind_method(D_METHOD("get_direction_priorities"), &IKEffectorTemplate3D::get_direction_priorities); + ClassDB::bind_method(D_METHOD("set_direction_priorities", "direction_priorities"), &IKEffectorTemplate3D::set_direction_priorities); + + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "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"); + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_node"), "set_target_node", "get_target_node"); + ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "root_bone"), "set_root_bone", "get_root_bone"); +} + +NodePath IKEffectorTemplate3D::get_target_node() const { + return target_node; +} + +void IKEffectorTemplate3D::set_target_node(NodePath p_node_path) { + target_node = p_node_path; +} + +float IKEffectorTemplate3D::get_motion_propagation_factor() const { + return motion_propagation_factor; +} + +void IKEffectorTemplate3D::set_motion_propagation_factor(float p_motion_propagation_factor) { + motion_propagation_factor = p_motion_propagation_factor; +} + +IKEffectorTemplate3D::IKEffectorTemplate3D() { +} + +String IKEffectorTemplate3D::get_root_bone() const { + return root_bone; +} + +void IKEffectorTemplate3D::set_root_bone(String p_node_path) { + root_bone = p_node_path; +} diff --git a/modules/many_bone_ik/src/ik_effector_template_3d.h b/modules/many_bone_ik/src/ik_effector_template_3d.h new file mode 100644 index 000000000000..a6f7add77f00 --- /dev/null +++ b/modules/many_bone_ik/src/ik_effector_template_3d.h @@ -0,0 +1,64 @@ +/**************************************************************************/ +/* ik_effector_template_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef IK_EFFECTOR_TEMPLATE_3D_H +#define IK_EFFECTOR_TEMPLATE_3D_H + +#include "core/io/resource.h" +#include "core/string/node_path.h" + +class IKEffectorTemplate3D : public Resource { + GDCLASS(IKEffectorTemplate3D, Resource); + + StringName root_bone; + NodePath target_node; + bool target_static = false; + real_t motion_propagation_factor = 1.0f; + real_t weight = 0.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); + NodePath get_target_node() const; + void set_target_node(NodePath p_node_path); + 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; } + + IKEffectorTemplate3D(); +}; + +#endif // IK_EFFECTOR_TEMPLATE_3D_H diff --git a/modules/many_bone_ik/src/ik_kusudama_3d.cpp b/modules/many_bone_ik/src/ik_kusudama_3d.cpp new file mode 100644 index 000000000000..d2bba623e197 --- /dev/null +++ b/modules/many_bone_ik/src/ik_kusudama_3d.cpp @@ -0,0 +1,427 @@ +/**************************************************************************/ +/* ik_kusudama_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "ik_kusudama_3d.h" + +#include "core/math/quaternion.h" +#include "ik_open_cone_3d.h" +#include "math/ik_node_3d.h" + +void IKKusudama3D::_update_constraint(Ref p_limiting_axes) { + // Avoiding antipodal singularities by reorienting the axes. + Vector directions; + + if (open_cones.size() == 1 && open_cones[0] != nullptr) { + directions.push_back(open_cones[0]->get_control_point()); + } else { + for (int i = 0; i < open_cones.size() - 1; i++) { + if (open_cones[i] == nullptr || open_cones[i + 1] == nullptr) { + continue; + } + + Vector3 this_control_point = open_cones[i]->get_control_point(); + Vector3 next_control_point = open_cones[i + 1]->get_control_point(); + + Quaternion this_to_next = Quaternion(this_control_point, next_control_point); + + Vector3 axis = this_to_next.get_axis(); + double angle = this_to_next.get_angle() / 2.0; + + Vector3 half_angle = this_control_point.rotated(axis, angle); + half_angle *= this_to_next.get_angle(); + half_angle.normalize(); + + directions.push_back(half_angle); + } + } + + Vector3 new_y; + for (Vector3 direction_vector : directions) { + new_y += direction_vector; + } + + if (!directions.is_empty()) { + new_y /= directions.size(); + new_y.normalize(); + } + + Transform3D new_y_ray = Transform3D(Basis(), new_y); + Quaternion old_y_to_new_y = Quaternion(p_limiting_axes->get_global_transform().get_basis().get_column(Vector3::AXIS_Y).normalized(), p_limiting_axes->get_global_transform().get_basis().xform(new_y_ray.origin).normalized()); + p_limiting_axes->rotate_local_with_global(old_y_to_new_y); + + for (Ref open_cone : open_cones) { + if (open_cone == nullptr) { + continue; + } + + Vector3 control_point = open_cone->get_control_point(); + open_cone->set_control_point(control_point.normalized()); + } + + update_tangent_radii(); +} + +void IKKusudama3D::update_tangent_radii() { + for (int i = 0; i < open_cones.size(); i++) { + Ref current = open_cones.write[i]; + Ref next; + if (i < open_cones.size() - 1) { + next = open_cones.write[i + 1]; + } + Ref cone = open_cones[i]; + cone->update_tangent_handles(next); + } +} + +void IKKusudama3D::set_axial_limits(real_t min_angle, real_t in_range) { + min_axial_angle = min_angle; + range_angle = in_range; + Vector3 y_axis = Vector3(0.0f, 1.0f, 0.0f); + Vector3 z_axis = Vector3(0.0f, 0.0f, 1.0f); + twist_min_rot = IKKusudama3D::get_quaternion_axis_angle(y_axis, min_axial_angle); + twist_min_vec = twist_min_rot.xform(z_axis).normalized(); + twist_center_vec = twist_min_rot.xform(twist_min_vec).normalized(); + twist_center_rot = Quaternion(z_axis, twist_center_vec); + twist_half_range_half_cos = Math::cos(in_range / real_t(4.0)); // For the quadrance angle. We need half the range angle since starting from the center, and half of that since quadrance takes cos(angle/2). + twist_max_vec = IKKusudama3D::get_quaternion_axis_angle(y_axis, in_range).xform(twist_min_vec).normalized(); + 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) { + if (!is_axially_constrained()) { + return; + } + Transform3D global_transform_constraint = p_constraint_axes->get_global_transform(); + Transform3D global_transform_to_set = p_to_set->get_global_transform(); + Basis parent_global_inverse = p_to_set->get_parent()->get_global_transform().basis.inverse(); + Basis global_twist_center = global_transform_constraint.basis * twist_center_rot; + 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 = 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)); +} + +void IKKusudama3D::get_swing_twist( + Quaternion p_rotation, + Vector3 p_axis, + Quaternion &r_swing, + Quaternion &r_twist) { +#ifdef MATH_CHECKS + ERR_FAIL_COND_MSG(!p_rotation.is_normalized(), "The quaternion must be normalized."); +#endif + if (Math::is_zero_approx(p_axis.length_squared())) { + r_swing = Quaternion(); + r_twist = Quaternion(); + return; + } + Quaternion rotation = p_rotation; + if (rotation.w < real_t(0.0)) { + rotation *= -1; + } + Vector3 p = p_axis * (rotation.x * p_axis.x + rotation.y * p_axis.y + rotation.z * p_axis.z); + r_twist = Quaternion(p.x, p.y, p.z, rotation.w).normalized(); + real_t d = Vector3(r_twist.x, r_twist.y, r_twist.z).dot(p_axis); + if (d < real_t(0.0)) { + r_twist *= real_t(-1.0); + } + r_swing = (rotation * r_twist.inverse()).normalized(); +} + +void IKKusudama3D::add_open_cone( + Ref p_cone) { + ERR_FAIL_COND(p_cone.is_null()); + ERR_FAIL_COND(p_cone->get_attached_to().is_null()); + open_cones.push_back(p_cone); + update_tangent_radii(); +} + +void IKKusudama3D::remove_open_cone(Ref limitCone) { + ERR_FAIL_COND(limitCone.is_null()); + open_cones.erase(limitCone); +} + +real_t IKKusudama3D::get_min_axial_angle() { + return min_axial_angle; +} + +real_t IKKusudama3D::get_range_angle() { + return range_angle; +} + +bool IKKusudama3D::is_axially_constrained() { + return axially_constrained; +} + +bool IKKusudama3D::is_orientationally_constrained() { + return orientationally_constrained; +} + +void IKKusudama3D::disable_orientational_limits() { + orientationally_constrained = false; +} + +void IKKusudama3D::enable_orientational_limits() { + orientationally_constrained = true; +} + +void IKKusudama3D::toggle_orientational_limits() { + orientationally_constrained = !orientationally_constrained; +} + +void IKKusudama3D::disable_axial_limits() { + axially_constrained = false; +} + +void IKKusudama3D::enable_axial_limits() { + axially_constrained = true; +} + +void IKKusudama3D::toggle_axial_limits() { + axially_constrained = !axially_constrained; +} + +bool IKKusudama3D::is_enabled() { + return axially_constrained || orientationally_constrained; +} + +void IKKusudama3D::disable() { + axially_constrained = false; + orientationally_constrained = false; +} + +void IKKusudama3D::enable() { + axially_constrained = true; + orientationally_constrained = true; +} + +TypedArray IKKusudama3D::get_open_cones() const { + TypedArray cones; + for (Ref cone : open_cones) { + cones.append(cone); + } + return cones; +} + +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(); + Vector3 result = point; + + if (open_cones.size() == 1) { + Ref cone = open_cones[0]; + result = cone->get_control_point(); + } else { + for (int i = 0; i < open_cones.size() - 1; i++) { + Ref next_cone = open_cones[i + 1]; + Ref cone = open_cones[i]; + Vector3 closestPathPoint = cone->get_closest_path_point(next_cone, point); + double closeDot = closestPathPoint.dot(point); + if (closeDot > closest_point_dot) { + result = closestPathPoint; + closest_point_dot = closeDot; + } + } + } + + return result; +} + +/** + * Given a point (in global coordinates), checks to see if a ray can be extended from the Kusudama's + * origin to that point, such that the ray in the Kusudama's reference frame is within the range_angle allowed by the Kusudama's + * coneLimits. + * If such a ray exists, the original point is returned (the point is within the limits). + * If it cannot exist, the tip of the ray within the kusudama's limits that would require the least rotation + * to arrive at the input point is returned. + * @param in_point the point to test. + * @param in_bounds returns a number from -1 to 1 representing the point's distance from the boundary, 0 means the point is right on + * the boundary, 1 means the point is within the boundary and on the path furthest from the boundary. any negative number means + * the point is outside of the boundary, but does not signify anything about how far from the boundary the point is. + * @return the original point, if it's in limits, or the closest point which is in limits. + */ +Vector3 IKKusudama3D::get_local_point_in_limits(Vector3 in_point, Vector *in_bounds) { + // Normalize the input point + Vector3 point = in_point.normalized(); + real_t closest_cos = -2.0; + in_bounds->write[0] = -1; + + Vector3 closest_collision_point = in_point; + + // Loop through each limit cone + for (int i = 0; i < open_cones.size(); i++) { + Ref cone = open_cones[i]; + Vector3 collision_point = cone->closest_to_cone(point, in_bounds); + + // If the collision point is NaN, return the original point + if (Math::is_nan(collision_point.x) || Math::is_nan(collision_point.y) || Math::is_nan(collision_point.z)) { + in_bounds->write[0] = 1; + return point; + } + + // Calculate the cosine of the angle between the collision point and the original point + real_t this_cos = collision_point.dot(point); + + // If the closest collision point is not set or the cosine is greater than the current closest cosine, update the closest collision point and cosine + if (closest_collision_point.is_zero_approx() || this_cos > closest_cos) { + closest_collision_point = collision_point; + closest_cos = this_cos; + } + } + + // If we're out of bounds of all cones, check if we're in the paths between the cones + if ((*in_bounds)[0] == -1) { + for (int i = 0; i < open_cones.size() - 1; i++) { + Ref currCone = open_cones[i]; + Ref nextCone = open_cones[i + 1]; + Vector3 collision_point = currCone->get_on_great_tangent_triangle(nextCone, point); + + // If the collision point is NaN, skip to the next iteration + if (Math::is_nan(collision_point.x)) { + continue; + } + + real_t this_cos = collision_point.dot(point); + + // If the cosine is approximately 1, return the original point + if (Math::is_equal_approx(this_cos, real_t(1.0))) { + in_bounds->write[0] = 1; + return point; + } + + // If the cosine is greater than the current closest cosine, update the closest collision point and cosine + if (this_cos > closest_cos) { + closest_collision_point = collision_point; + closest_cos = this_cos; + } + } + } + + // Return the closest boundary point between cones + return closest_collision_point; +} + +void IKKusudama3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_open_cones"), &IKKusudama3D::get_open_cones); + ClassDB::bind_method(D_METHOD("set_open_cones", "open_cones"), &IKKusudama3D::set_open_cones); +} + +void IKKusudama3D::set_open_cones(TypedArray p_cones) { + open_cones.clear(); + open_cones.resize(p_cones.size()); + for (int32_t i = 0; i < p_cones.size(); i++) { + open_cones.write[i] = p_cones[i]; + } +} + +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; + } + if (to_set.is_null()) { + return; + } + if (limiting_axes.is_null()) { + return; + } + Vector in_bounds; + in_bounds.resize(1); + in_bounds.write[0] = 1.0; + Vector3 limiting_origin = limiting_axes->get_global_transform().origin; + Vector3 bone_dir_xform = bone_direction->get_global_transform().xform(Vector3(0.0, 1.0, 0.0)); + + bone_ray->set_point_1(limiting_origin); + bone_ray->set_point_2(bone_dir_xform); + + Vector3 bone_tip = limiting_axes->to_local(bone_ray->get_point_2()); + Vector3 in_limits = get_local_point_in_limits(bone_tip, &in_bounds); + + if (in_bounds[0] < 0) { + constrained_ray->set_point_1(bone_ray->get_point_1()); + constrained_ray->set_point_2(limiting_axes->to_global(in_limits)); + + Quaternion rectified_rot = Quaternion(bone_ray->get_heading(), constrained_ray->get_heading()); + to_set->rotate_local_with_global(rectified_rot); + } +} + +bool IKKusudama3D::is_nan_vector(const Vector3 &vec) { + return Math::is_nan(vec.x) || Math::is_nan(vec.y) || Math::is_nan(vec.z); +} + +void IKKusudama3D::set_resistance(float p_resistance) { + resistance = p_resistance; +} + +float IKKusudama3D::get_resistance() { + return resistance; +} + +Quaternion IKKusudama3D::clamp_to_quadrance_angle(Quaternion p_rotation, double p_cos_half_angle) { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!p_rotation.is_normalized(), Quaternion(), "The quaternion must be normalized."); +#endif + Quaternion rotation = p_rotation; + double newCoeff = 1.0 - (p_cos_half_angle * abs(p_cos_half_angle)); + double currentCoeff = rotation.x * rotation.x + rotation.y * rotation.y + rotation.z * rotation.z; + if (newCoeff >= currentCoeff) { + return rotation; + } + double over_limit = (currentCoeff - newCoeff) / (1.0 - newCoeff); + Quaternion clamped_rotation = rotation; + clamped_rotation.w = rotation.w < 0 ? -p_cos_half_angle : p_cos_half_angle; + double compositeCoeff = sqrt(newCoeff / currentCoeff); + clamped_rotation.x *= compositeCoeff; + clamped_rotation.y *= compositeCoeff; + clamped_rotation.z *= compositeCoeff; + if (!rotation.is_finite() || !clamped_rotation.is_finite()) { + return Quaternion(); + } + return rotation.slerp(clamped_rotation, over_limit); +} + +void IKKusudama3D::clear_open_cones() { + open_cones.clear(); +} + +Quaternion IKKusudama3D::get_quaternion_axis_angle(const Vector3 &p_axis, real_t p_angle) { + real_t d = p_axis.length_squared(); + if (d == 0) { + return Quaternion(); + } else { + real_t sin_angle = Math::sin(p_angle * 0.5f); + real_t cos_angle = Math::cos(p_angle * 0.5f); + real_t s = sin_angle / d; + return Quaternion(p_axis.x * s, p_axis.y * s, p_axis.z * s, cos_angle); + } +} diff --git a/modules/many_bone_ik/src/ik_kusudama_3d.h b/modules/many_bone_ik/src/ik_kusudama_3d.h new file mode 100644 index 000000000000..0ef9a926d85a --- /dev/null +++ b/modules/many_bone_ik/src/ik_kusudama_3d.h @@ -0,0 +1,203 @@ +/**************************************************************************/ +/* ik_kusudama_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef IK_KUSUDAMA_3D_H +#define IK_KUSUDAMA_3D_H + +#include "ik_bone_3d.h" +#include "ik_bone_segment_3d.h" +#include "ik_open_cone_3d.h" +#include "ik_ray_3d.h" +#include "math/ik_node_3d.h" + +#include "core/io/resource.h" +#include "core/math/quaternion.h" +#include "core/object/ref_counted.h" +#include "core/variant/typed_array.h" +#include "scene/3d/node_3d.h" + +class IKBone3D; +class IKLimitCone3D; +class IKKusudama3D : public Resource { + GDCLASS(IKKusudama3D, Resource); + + /** + * An array containing all of the Kusudama's open_cones. The kusudama is built up + * with the expectation that any limitCone in the array is connected to the cone at the previous element in the array, + * and the cone at the next element in the array. + */ + Vector> open_cones; + + Quaternion twist_min_rot; + Vector3 twist_min_vec; + Vector3 twist_max_vec; + Vector3 twist_center_vec; + Quaternion twist_center_rot; + Quaternion twist_max_rot; + real_t twist_half_range_half_cos = 0; + Vector3 twist_tan; + bool flipped_bounds = false; + real_t resistance = 0; + + /** + * Defined as some Angle in radians about the limiting_axes Y axis, 0 being equivalent to the + * limiting_axes Z axis. + */ + real_t min_axial_angle = 0.0; + /** + * Defined as some Angle in radians about the limiting_axes Y axis, 0 being equivalent to the + * min_axial_angle + */ + real_t range_angle = Math_TAU; + + bool orientationally_constrained = false; + bool axially_constrained = false; + +protected: + static void _bind_methods(); + +public: + ~IKKusudama3D() {} + + IKKusudama3D() {} + + void _update_constraint(Ref p_limiting_axes); + + void update_tangent_radii(); + + Ref bone_ray = Ref(memnew(IKRay3D())); + Ref constrained_ray = Ref(memnew(IKRay3D())); + double unit_hyper_area = 2 * Math::pow(Math_PI, 2); + double unit_area = 4 * Math_PI; + + /** + * Get the swing rotation and twist rotation for the specified axis. The twist rotation represents the rotation around the specified axis. The swing rotation represents the rotation of the specified + * axis itself, which is the rotation around an axis perpendicular to the specified axis. The swing and twist rotation can be + * used to reconstruct the original quaternion: this = swing * twist + * + * @param p_axis the X, Y, Z component of the normalized axis for which to get the swing and twist rotation + * @return twist represent the rotational twist + * @return swing represent the rotational swing + * @see calculation + */ + static void get_swing_twist( + Quaternion p_rotation, + Vector3 p_axis, + Quaternion &r_swing, + Quaternion &r_twist); + + static Quaternion get_quaternion_axis_angle(const Vector3 &p_axis, real_t p_angle); + +public: + /** + * Presumes the input axes are the bone's localAxes, and rotates + * them to satisfy the snap limits. + * + * @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); + + bool is_nan_vector(const Vector3 &vec); + + /** + * Kusudama constraints decompose the bone orientation into a swing component, and a twist component. + * The "Swing" component is the final direction of the bone. The "Twist" component represents how much + * the bone is rotated about its own final direction. Where limit cones allow you to constrain the "Swing" + * component, this method lets you constrain the "twist" component. + * + * @param min_angle some angle in radians about the major rotation frame's y-axis to serve as the first angle within the range_angle that the bone is allowed to twist. + * @param in_range some angle in radians added to the min_angle. if the bone's local Z goes maxAngle radians beyond the min_angle, it is considered past the limit. + * This value is always interpreted as being in the positive direction. For example, if this value is -PI/2, the entire range_angle from min_angle to min_angle + 3PI/4 is + * considered valid. + */ + void set_axial_limits(real_t p_min_angle, real_t p_in_range); + + /** + * + * @param to_set + * @param limiting_axes + * @return radians of the twist required to snap bone into twist limits (0 if bone is already in twist limits) + */ + void set_snap_to_twist_limit(Ref 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 + * origin to that point, such that the ray in the Kusudama's reference frame is within the range_angle allowed by the Kusudama's + * coneLimits. + * If such a ray exists, the original point is returned (the point is within the limits). + * If it cannot exist, the tip of the ray within the kusudama's limits that would require the least rotation + * to arrive at the input point is returned. + * @param in_point the point to test. + * @param in_bounds should be an array with at least 2 elements. The first element will be set to a number from -1 to 1 representing the point's distance from the boundary, 0 means the point is right on + * the boundary, 1 means the point is within the boundary and on the path furthest from the boundary. any negative number means + * the point is outside of the boundary, but does not signify anything about how far from the boundary the point is. + * The second element will be given a value corresponding to the limit cone whose bounds were exceeded. If the bounds were exceeded on a segment between two limit cones, + * this value will be set to a non-integer value between the two indices of the limitcone comprising the segment whose bounds were exceeded. + * @return the original point, if it's in limits, or the closest point which is in limits. + */ + Vector3 get_local_point_in_limits(Vector3 in_point, Vector *in_bounds); + + Vector3 local_point_on_path_sequence(Vector3 in_point, Ref limiting_axes); + + /** + * Add a IKLimitCone to the Kusudama. + * @param new_point where on the Kusudama to add the LimitCone (in Kusudama's local coordinate frame defined by its bone's majorRotationAxes)) + * @param radius the radius of the limitCone + */ + void add_open_cone(Ref p_open_cone); + void remove_open_cone(Ref limitCone); + + /** + * + * @return the lower bound on the axial constraint + */ + real_t get_min_axial_angle(); + real_t get_range_angle(); + + bool is_axially_constrained(); + bool is_orientationally_constrained(); + void disable_orientational_limits(); + void enable_orientational_limits(); + void toggle_orientational_limits(); + void disable_axial_limits(); + void enable_axial_limits(); + void toggle_axial_limits(); + bool is_enabled(); + void disable(); + void enable(); + void clear_open_cones(); + TypedArray get_open_cones() const; + void set_open_cones(TypedArray p_cones); + float get_resistance(); + void set_resistance(float p_resistance); + static Quaternion clamp_to_quadrance_angle(Quaternion p_rotation, double p_cos_half_angle); +}; + +#endif // IK_KUSUDAMA_3D_H diff --git a/modules/many_bone_ik/src/ik_open_cone_3d.cpp b/modules/many_bone_ik/src/ik_open_cone_3d.cpp new file mode 100644 index 000000000000..b4d3b3997e57 --- /dev/null +++ b/modules/many_bone_ik/src/ik_open_cone_3d.cpp @@ -0,0 +1,426 @@ +/**************************************************************************/ +/* ik_open_cone_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "ik_open_cone_3d.h" + +#include "core/math/quaternion.h" +#include "ik_kusudama_3d.h" + +void IKLimitCone3D::update_tangent_handles(Ref p_next) { + if (p_next.is_null()) { + return; + } + double radA = get_radius(); + double radB = p_next->get_radius(); + + Vector3 A = get_control_point(); + Vector3 B = p_next->get_control_point(); + + Vector3 arc_normal = A.cross(B).normalized(); + + /** + * There are an infinite number of circles co-tangent with A and B, every other + * one of which has a unique radius. + * + * However, we want the radius of our tangent circles to obey the following properties: + * 1) When the radius of A + B == 0, our tangent circle's radius should = 90. + * In other words, the tangent circle should span a hemisphere. + * 2) When the radius of A + B == 180, our tangent circle's radius should = 0. + * In other words, when A + B combined are capable of spanning the entire sphere, + * our tangentCircle should be nothing. + * + * Another way to think of this is -- whatever the maximum distance can be between the + * borders of A and B (presuming their centers are free to move about the circle + * but their radii remain constant), we want our tangentCircle's diameter to be precisely that distance, + * and so, our tangent circles radius should be precisely half of that distance. + */ + double tRadius = (Math_PI - (radA + radB)) / 2; + + /** + * Once we have the desired radius for our tangent circle, we may find the solution for its + * centers (usually, there are two). + */ + double boundaryPlusTangentRadiusA = radA + tRadius; + double boundaryPlusTangentRadiusB = radB + tRadius; + + // the axis of this cone, scaled to minimize its distance to the tangent contact points. + Vector3 scaledAxisA = A * Math::cos(boundaryPlusTangentRadiusA); + // a point on the plane running through the tangent contact points + Quaternion temp_var = IKKusudama3D::get_quaternion_axis_angle(arc_normal, boundaryPlusTangentRadiusA); + Vector3 planeDir1A = temp_var.xform(A); + // another point on the same plane + Quaternion tempVar2 = IKKusudama3D::get_quaternion_axis_angle(A, Math_PI / 2); + Vector3 planeDir2A = tempVar2.xform(planeDir1A); + + Vector3 scaledAxisB = B * cos(boundaryPlusTangentRadiusB); + // a point on the plane running through the tangent contact points + Quaternion tempVar3 = IKKusudama3D::get_quaternion_axis_angle(arc_normal, boundaryPlusTangentRadiusB); + Vector3 planeDir1B = tempVar3.xform(B); + // another point on the same plane + Quaternion tempVar4 = IKKusudama3D::get_quaternion_axis_angle(B, Math_PI / 2); + Vector3 planeDir2B = tempVar4.xform(planeDir1B); + + // ray from scaled center of next cone to half way point between the circumference of this cone and the next cone. + Ref r1B = Ref(memnew(IKRay3D(planeDir1B, scaledAxisB))); + Ref r2B = Ref(memnew(IKRay3D(planeDir1B, planeDir2B))); + + r1B->elongate(99); + r2B->elongate(99); + + Vector3 intersection1 = r1B->get_intersects_plane(scaledAxisA, planeDir1A, planeDir2A); + Vector3 intersection2 = r2B->get_intersects_plane(scaledAxisA, planeDir1A, planeDir2A); + + Ref intersectionRay = Ref(memnew(IKRay3D(intersection1, intersection2))); + intersectionRay->elongate(99); + + Vector3 sphereIntersect1; + Vector3 sphereIntersect2; + Vector3 sphereCenter; + intersectionRay->intersects_sphere(sphereCenter, 1.0f, &sphereIntersect1, &sphereIntersect2); + + set_tangent_circle_center_next_1(sphereIntersect1); + set_tangent_circle_center_next_2(sphereIntersect2); + set_tangent_circle_radius_next(tRadius); + if (Math::is_zero_approx(tangent_circle_center_next_1.length_squared())) { + tangent_circle_center_next_1 = get_orthogonal(control_point).normalized(); + } + if (Math::is_zero_approx(tangent_circle_center_next_2.length_squared())) { + tangent_circle_center_next_2 = get_orthogonal(tangent_circle_center_next_1 * -1).normalized(); + } + if (p_next.is_valid()) { + compute_triangles(p_next); + } +} + +void IKLimitCone3D::set_tangent_circle_radius_next(double rad) { + tangent_circle_radius_next = rad; + tangent_circle_radius_next_cos = cos(tangent_circle_radius_next); +} + +Vector3 IKLimitCone3D::get_tangent_circle_center_next_1() { + return tangent_circle_center_next_1; +} + +double IKLimitCone3D::get_tangent_circle_radius_next() { + return tangent_circle_radius_next; +} + +double IKLimitCone3D::_get_tangent_circle_radius_next_cos() { + return tangent_circle_radius_next_cos; +} + +Vector3 IKLimitCone3D::get_tangent_circle_center_next_2() { + return tangent_circle_center_next_2; +} + +void IKLimitCone3D::compute_triangles(Ref p_next) { + if (p_next.is_null()) { + return; + } + first_triangle_next.write[1] = tangent_circle_center_next_1.normalized(); + first_triangle_next.write[0] = get_control_point().normalized(); + first_triangle_next.write[2] = p_next->get_control_point().normalized(); + + second_triangle_next.write[1] = tangent_circle_center_next_2.normalized(); + second_triangle_next.write[0] = get_control_point().normalized(); + second_triangle_next.write[2] = p_next->get_control_point().normalized(); +} + +Vector3 IKLimitCone3D::get_control_point() const { + return control_point; +} + +void IKLimitCone3D::set_control_point(Vector3 p_control_point) { + if (Math::is_zero_approx(p_control_point.length_squared())) { + control_point = Vector3(0, 1, 0); + } else { + control_point = p_control_point; + control_point.normalize(); + } +} + +double IKLimitCone3D::get_radius() const { + return radius; +} + +double IKLimitCone3D::get_radius_cosine() const { + return radius_cosine; +} + +void IKLimitCone3D::set_radius(double p_radius) { + radius = p_radius; + radius_cosine = cos(p_radius); +} + +bool IKLimitCone3D::_determine_if_in_bounds(Ref next, Vector3 input) const { + /** + * Procedure : Check if input is contained in this cone, or the next cone + * if it is, then we're finished and in bounds. otherwise, + * check if the point is contained within the tangent radii, + * if it is, then we're out of bounds and finished, otherwise + * in the tangent triangles while still remaining outside of the tangent radii + * if it is, then we're finished and in bounds. otherwise, we're out of bounds. + */ + + if (control_point.dot(input) >= radius_cosine) { + return true; + } else if (next.is_valid() && next->control_point.dot(input) >= next->radius_cosine) { + return true; + } else { + if (next.is_null()) { + return false; + } + bool inTan1Rad = tangent_circle_center_next_1.dot(input) > tangent_circle_radius_next_cos; + if (inTan1Rad) { + return false; + } + bool inTan2Rad = tangent_circle_center_next_2.dot(input) > tangent_circle_radius_next_cos; + if (inTan2Rad) { + return false; + } + + /*if we reach this point in the code, we are either on the path between two open_cones, or on the path extending out from between them + * but outside of their radii. + * To determine which , we take the cross product of each control point with each tangent center. + * The direction of each of the resultant vectors will represent the normal of a plane. + * Each of these four planes define part of a boundary which determines if our point is in bounds. + * If the dot product of our point with the normal of any of these planes is negative, we must be out + * of bounds. + * + * Older version of this code relied on a triangle intersection algorithm here, which I think is slightly less efficient on average + * as it didn't allow for early termination. . + */ + + Vector3 c1xc2 = control_point.cross(next->control_point); + double c1c2dir = input.dot(c1xc2); + + if (c1c2dir < 0.0) { + Vector3 c1xt1 = control_point.cross(tangent_circle_center_next_1); + Vector3 t1xc2 = tangent_circle_center_next_1.cross(next->control_point); + return input.dot(c1xt1) > 0 && input.dot(t1xc2) > 0; + } else { + Vector3 t2xc1 = tangent_circle_center_next_2.cross(control_point); + Vector3 c2xt2 = next->control_point.cross(tangent_circle_center_next_2); + return input.dot(t2xc1) > 0 && input.dot(c2xt2) > 0; + } + } +} + +Vector3 IKLimitCone3D::get_closest_path_point(Ref next, Vector3 input) const { + Vector3 result; + if (next.is_null()) { + result = _closest_cone(Ref(this), input); + } else { + result = _get_on_path_sequence(next, input); + bool is_number = !(Math::is_nan(result.x) && Math::is_nan(result.y) && Math::is_nan(result.z)); + if (!is_number) { + result = _closest_cone(next, input); + } + } + return result; +} + +Vector3 IKLimitCone3D::_get_closest_collision(Ref next, Vector3 input) const { + ERR_FAIL_COND_V(next.is_null(), input); + Vector3 result; + if (next.is_null()) { + Vector in_bounds = { 0.0 }; + result = _closest_cone(Ref(), input); + } else { + result = get_on_great_tangent_triangle(next, input); + bool is_number = !(Math::is_nan(result.x) && Math::is_nan(result.y) && Math::is_nan(result.z)); + if (!is_number) { + Vector in_bounds = { 0.0 }; + result = _closest_point_on_closest_cone(next, input, &in_bounds); + } + } + return result; +} + +Vector3 IKLimitCone3D::get_orthogonal(Vector3 p_in) { + Vector3 result; + float threshold = p_in.length() * 0.6f; + if (threshold > 0.f) { + if (Math::abs(p_in.x) <= threshold) { + float inverse = 1.f / Math::sqrt(p_in.y * p_in.y + p_in.z * p_in.z); + return result = Vector3(0.f, inverse * p_in.z, -inverse * p_in.y); + } else if (Math::abs(p_in.y) <= threshold) { + float inverse = 1.f / Math::sqrt(p_in.x * p_in.x + p_in.z * p_in.z); + return result = Vector3(-inverse * p_in.z, 0.f, inverse * p_in.x); + } + float inverse = 1.f / Math::sqrt(p_in.x * p_in.x + p_in.y * p_in.y); + return result = Vector3(inverse * p_in.y, -inverse * p_in.x, 0.f); + } + + return result; +} + +Vector3 IKLimitCone3D::get_on_great_tangent_triangle(Ref next, Vector3 input) const { + ERR_FAIL_COND_V(next.is_null(), input); + Vector3 c1xc2 = control_point.cross(next->control_point); + double c1c2dir = input.dot(c1xc2); + if (c1c2dir < 0.0) { + Vector3 c1xt1 = control_point.cross(tangent_circle_center_next_1).normalized(); + Vector3 t1xc2 = tangent_circle_center_next_1.cross(next->control_point).normalized(); + if (input.dot(c1xt1) > 0 && input.dot(t1xc2) > 0) { + double to_next_cos = input.dot(tangent_circle_center_next_1); + if (to_next_cos > tangent_circle_radius_next_cos) { + Vector3 plane_normal = tangent_circle_center_next_1.cross(input).normalized(); + plane_normal.normalize(); + Quaternion rotate_about_by = Quaternion(plane_normal, tangent_circle_radius_next); + return rotate_about_by.xform(tangent_circle_center_next_1); + } else { + return input; + } + } else { + return Vector3(NAN, NAN, NAN); + } + } else { + Vector3 t2xc1 = tangent_circle_center_next_2.cross(control_point).normalized(); + Vector3 c2xt2 = next->control_point.cross(tangent_circle_center_next_2).normalized(); + if (input.dot(t2xc1) > 0 && input.dot(c2xt2) > 0) { + if (input.dot(tangent_circle_center_next_2) > tangent_circle_radius_next_cos) { + Vector3 plane_normal = tangent_circle_center_next_2.cross(input).normalized(); + plane_normal.normalize(); + Quaternion rotate_about_by = Quaternion(plane_normal, tangent_circle_radius_next); + return rotate_about_by.xform(tangent_circle_center_next_2); + } else { + return input; + } + } else { + return Vector3(NAN, NAN, NAN); + } + } +} + +Vector3 IKLimitCone3D::_closest_cone(Ref next, Vector3 input) const { + if (next.is_null()) { + return control_point; + } + if (input.dot(control_point) > input.dot(next->control_point)) { + return control_point; + } else { + return next->control_point; + } +} + +Vector3 IKLimitCone3D::_closest_point_on_closest_cone(Ref next, Vector3 input, Vector *in_bounds) const { + ERR_FAIL_COND_V(next.is_null(), input); + Vector3 closestToFirst = closest_to_cone(input, in_bounds); + if (in_bounds != nullptr && (*in_bounds)[0] > 0.0) { + return closestToFirst; + } + if (next.is_null()) { + return closestToFirst; + } else { + Vector3 closestToSecond = next->closest_to_cone(input, in_bounds); + if (in_bounds != nullptr && (*in_bounds)[0] > 0.0) { + return closestToSecond; + } + double cosToFirst = input.dot(closestToFirst); + double cosToSecond = input.dot(closestToSecond); + + if (cosToFirst > cosToSecond) { + return closestToFirst; + } else { + return closestToSecond; + } + } +} + +Vector3 IKLimitCone3D::closest_to_cone(Vector3 input, Vector *in_bounds) const { + Vector3 normalized_input = input.normalized(); + Vector3 normalized_control_point = get_control_point().normalized(); + if (normalized_input.dot(normalized_control_point) > get_radius_cosine()) { + if (in_bounds != nullptr) { + in_bounds->write[0] = 1.0; + } + return Vector3(NAN, NAN, NAN); + } + Vector3 axis = normalized_control_point.cross(normalized_input).normalized(); + if (Math::is_zero_approx(axis.length_squared()) || !axis.is_finite()) { + axis = Vector3(0, 1, 0); + } + Quaternion rot_to = IKKusudama3D::get_quaternion_axis_angle(axis, get_radius()); + Vector3 axis_control_point = normalized_control_point; + if (Math::is_zero_approx(axis_control_point.length_squared())) { + axis_control_point = Vector3(0, 1, 0); + } + Vector3 result = rot_to.xform(axis_control_point); + if (in_bounds != nullptr) { + in_bounds->write[0] = -1; + } + return result; +} + +void IKLimitCone3D::set_tangent_circle_center_next_1(Vector3 point) { + tangent_circle_center_next_1 = point.normalized(); +} + +void IKLimitCone3D::set_tangent_circle_center_next_2(Vector3 point) { + tangent_circle_center_next_2 = point.normalized(); +} + +Vector3 IKLimitCone3D::_get_on_path_sequence(Ref next, Vector3 input) const { + if (next.is_null()) { + return Vector3(NAN, NAN, NAN); + } + Vector3 c1xc2 = get_control_point().cross(next->control_point).normalized(); + double c1c2dir = input.dot(c1xc2); + if (c1c2dir < 0.0) { + Vector3 c1xt1 = get_control_point().cross(tangent_circle_center_next_1).normalized(); + Vector3 t1xc2 = tangent_circle_center_next_1.cross(next->get_control_point()).normalized(); + if (input.dot(c1xt1) > 0.0f && input.dot(t1xc2) > 0.0f) { + Ref tan1ToInput = Ref(memnew(IKRay3D(tangent_circle_center_next_1, input))); + Vector3 result = tan1ToInput->get_intersects_plane(Vector3(0.0f, 0.0f, 0.0f), get_control_point(), next->get_control_point()); + return result.normalized(); + } else { + return Vector3(NAN, NAN, NAN); + } + } else { + Vector3 t2xc1 = tangent_circle_center_next_2.cross(control_point).normalized(); + Vector3 c2xt2 = next->get_control_point().cross(tangent_circle_center_next_2).normalized(); + if (input.dot(t2xc1) > 0 && input.dot(c2xt2) > 0) { + Ref tan2ToInput = Ref(memnew(IKRay3D(tangent_circle_center_next_2, input))); + Vector3 result = tan2ToInput->get_intersects_plane(Vector3(0.0f, 0.0f, 0.0f), get_control_point(), next->get_control_point()); + return result.normalized(); + } else { + return Vector3(NAN, NAN, NAN); + } + } +} + +void IKLimitCone3D::set_attached_to(Ref p_attached_to) { + parent_kusudama.set_ref(p_attached_to); +} + +Ref IKLimitCone3D::get_attached_to() { + return parent_kusudama.get_ref(); +} diff --git a/modules/many_bone_ik/src/ik_open_cone_3d.h b/modules/many_bone_ik/src/ik_open_cone_3d.h new file mode 100644 index 000000000000..47bb31b4e469 --- /dev/null +++ b/modules/many_bone_ik/src/ik_open_cone_3d.h @@ -0,0 +1,135 @@ +/**************************************************************************/ +/* ik_open_cone_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef IK_OPEN_CONE_3D_H +#define IK_OPEN_CONE_3D_H + +#include "core/io/resource.h" +#include "core/math/vector3.h" +#include "core/object/ref_counted.h" + +class IKKusudama3D; +class IKLimitCone3D : public Resource { + GDCLASS(IKLimitCone3D, Resource); + void compute_triangles(Ref p_next); + + Vector3 control_point = Vector3(0, 1, 0); + Vector3 radial_point; + + // Radius stored as cosine to save on the acos call necessary for the angle between. + double radius_cosine = 0; + double radius = 0; + Vector3 _closest_cone(Ref next, Vector3 input) const; + void set_tangent_circle_radius_next(double rad); + WeakRef parent_kusudama; + + Vector3 tangent_circle_center_next_1; + Vector3 tangent_circle_center_next_2; + double tangent_circle_radius_next = 0; + double tangent_circle_radius_next_cos = 0; + + /** + * A triangle where the [1] is the tangent_circle_next_n, and [0] and [2] + * are the points at which the tangent circle intersects this IKLimitCone and the + * next IKLimitCone. + */ + Vector first_triangle_next = { Vector3(), Vector3(), Vector3() }; + Vector second_triangle_next = { Vector3(), Vector3(), Vector3() }; + + /** + * + * @param next + * @param input + * @return null if the input point is already in bounds, or the point's rectified position + * if the point was out of bounds. + */ + Vector3 _get_closest_collision(Ref next, Vector3 input) const; + + /** + * Determines if a ray emanating from the origin to given point in local space + * lies within the path from this cone to the next cone. This function relies on + * an optimization trick for a performance boost, but the trick ruins everything + * if the input isn't normalized. So it is ABSOLUTELY VITAL + * that @param input have unit length in order for this function to work correctly. + * @param next + * @param input + * @return + */ + bool _determine_if_in_bounds(Ref next, Vector3 input) const; + Vector3 _get_on_path_sequence(Ref next, Vector3 input) const; + + /** + * returns null if no rectification is required. + * @param next + * @param input + * @param in_bounds + * @return + */ + Vector3 _closest_point_on_closest_cone(Ref next, Vector3 input, Vector *in_bounds) const; + + double _get_tangent_circle_radius_next_cos(); + +public: + IKLimitCone3D() {} + virtual ~IKLimitCone3D() {} + void set_attached_to(Ref p_attached_to); + Ref get_attached_to(); + void update_tangent_handles(Ref p_next); + void set_tangent_circle_center_next_1(Vector3 point); + void set_tangent_circle_center_next_2(Vector3 point); + /** + * + * @param next + * @param input + * @return null if inapplicable for rectification. the original point if in bounds, or the point rectified to the closest boundary on the path sequence + * between two cones if the point is out of bounds and applicable for rectification. + */ + Vector3 get_on_great_tangent_triangle(Ref next, Vector3 input) const; + double get_tangent_circle_radius_next(); + Vector3 get_tangent_circle_center_next_1(); + Vector3 get_tangent_circle_center_next_2(); + + /** + * returns null if no rectification is required. + * @param input + * @param in_bounds + * @return + */ + Vector3 closest_to_cone(Vector3 input, Vector *in_bounds) const; + Vector3 get_closest_path_point(Ref next, Vector3 input) const; + Vector3 get_control_point() const; + void set_control_point(Vector3 p_control_point); + double get_radius() const; + double get_radius_cosine() const; + void set_radius(double radius); + static Vector3 get_orthogonal(Vector3 p_input); +}; + +#endif // IK_OPEN_CONE_3D_H diff --git a/modules/many_bone_ik/src/ik_ray_3d.cpp b/modules/many_bone_ik/src/ik_ray_3d.cpp new file mode 100644 index 000000000000..4ec35a56cc11 --- /dev/null +++ b/modules/many_bone_ik/src/ik_ray_3d.cpp @@ -0,0 +1,212 @@ +/**************************************************************************/ +/* ik_ray_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "ik_ray_3d.h" + +IKRay3D::IKRay3D() { +} + +IKRay3D::IKRay3D(Vector3 p_p1, Vector3 p_p2) { + working_vector = p_p1; + point_1 = p_p1; + point_2 = p_p2; +} + +Vector3 IKRay3D::get_heading() { + working_vector = point_2; + return working_vector - point_1; +} + +void IKRay3D::set_heading(const Vector3 &p_new_head) { + point_2 = point_1; + point_2 = p_new_head; +} + +real_t IKRay3D::get_scaled_projection(const Vector3 p_input) { + working_vector = p_input; + working_vector = working_vector - point_1; + Vector3 heading = get_heading(); + real_t headingMag = heading.length(); + real_t workingVectorMag = working_vector.length(); + if (workingVectorMag == 0 || headingMag == 0) { + return 0; + } + return (working_vector.dot(heading) / (headingMag * workingVectorMag)) * (workingVectorMag / headingMag); +} + +void IKRay3D::elongate(real_t amt) { + Vector3 midPoint = (point_1 + point_2) * 0.5f; + Vector3 p1Heading = point_1 - midPoint; + Vector3 p2Heading = point_2 - midPoint; + Vector3 p1Add = p1Heading.normalized() * amt; + Vector3 p2Add = p2Heading.normalized() * amt; + + point_1 = p1Heading + p1Add + midPoint; + point_2 = p2Heading + p2Add + midPoint; +} + +Vector3 IKRay3D::get_intersects_plane(Vector3 ta, Vector3 tb, Vector3 tc) { + Vector3 uvw; + tta = ta; + ttb = tb; + ttc = tc; + tta -= point_1; + ttb -= point_1; + ttc -= point_1; + Vector3 result = plane_intersect_test(tta, ttb, ttc, &uvw); + return result + point_1; +} + +int IKRay3D::intersects_sphere(Vector3 sphereCenter, real_t radius, Vector3 *S1, Vector3 *S2) { + Vector3 tp1 = point_1 - sphereCenter; + Vector3 tp2 = point_2 - sphereCenter; + int result = intersects_sphere(tp1, tp2, radius, S1, S2); + *S1 += sphereCenter; + *S2 += sphereCenter; + return result; +} + +void IKRay3D::set_point_1(Vector3 in) { + point_1 = in; +} + +void IKRay3D::set_point_2(Vector3 in) { + point_2 = in; +} + +Vector3 IKRay3D::get_point_2() { + return point_2; +} + +Vector3 IKRay3D::get_point_1() { + return point_1; +} + +int IKRay3D::intersects_sphere(Vector3 rp1, Vector3 rp2, real_t radius, Vector3 *S1, Vector3 *S2) { + Vector3 direction = rp2 - rp1; + Vector3 e = direction; // e=ray.dir + e.normalize(); // e=g/|g| + Vector3 h = point_1; + h = Vector3(0.0f, 0.0f, 0.0f); + h = h - rp1; // h=r.o-c.M + real_t lf = e.dot(h); // lf=e.h + real_t radpow = radius * radius; + real_t hdh = h.length_squared(); + real_t lfpow = lf * lf; + real_t s = radpow - hdh + lfpow; // s=r^2-h^2+lf^2 + if (s < 0.0f) { + return 0; // no intersection points ? + } + s = Math::sqrt(s); // s=sqrt(r^2-h^2+lf^2) + + int result = 0; + if (lf < s) { + if (lf + s >= 0) { + s = -s; // swap S1 <-> S2} + result = 1; // one intersection point + } + } else { + result = 2; // 2 intersection points + } + + *S1 = e * (lf - s); + *S1 += rp1; // S1=A+e*(lf-s) + *S2 = e * (lf + s); + *S2 += rp1; // S2=A+e*(lf+s) + return result; +} + +Vector3 IKRay3D::plane_intersect_test(Vector3 ta, Vector3 tb, Vector3 tc, Vector3 *uvw) { + u = tb; + v = tc; + n = Vector3(0, 0, 0); + dir = get_heading(); + w0 = Vector3(0, 0, 0); + real_t r, a, b; + u -= ta; + v -= ta; + + n = u.cross(v).normalized(); + + w0 -= ta; + a = -(n.dot(w0)); + b = n.dot(dir); + r = a / b; + I = dir; + I *= r; + barycentric(ta, tb, tc, I, uvw); + return I; +} + +real_t IKRay3D::triangle_area_2d(real_t x1, real_t y1, real_t x2, real_t y2, real_t x3, real_t y3) { + return (x1 - x2) * (y2 - y3) - (x2 - x3) * (y1 - y2); +} + +void IKRay3D::barycentric(Vector3 a, Vector3 b, Vector3 c, Vector3 p, Vector3 *uvw) { + bc = b; + ca = a; + at = a; + bt = b; + ct = c; + pt = p; + + m = Vector3(bc - ct).cross(ca - at).normalized(); + + real_t nu; + real_t nv; + real_t ood; + + real_t x = Math::abs(m.x); + real_t y = Math::abs(m.y); + real_t z = Math::abs(m.z); + + if (x >= y && x >= z) { + nu = triangle_area_2d(pt.y, pt.z, bt.y, bt.z, ct.y, ct.z); + nv = triangle_area_2d(pt.y, pt.z, ct.y, ct.z, at.y, at.z); + ood = 1.0f / m.x; + } else if (y >= x && y >= z) { + nu = triangle_area_2d(pt.x, pt.z, bt.x, bt.z, ct.x, ct.z); + nv = triangle_area_2d(pt.x, pt.z, ct.x, ct.z, at.x, at.z); + ood = 1.0f / -m.y; + } else { + nu = triangle_area_2d(pt.x, pt.y, bt.x, bt.y, ct.x, ct.y); + nv = triangle_area_2d(pt.x, pt.y, ct.x, ct.y, at.x, at.y); + ood = 1.0f / m.z; + } + (*uvw)[0] = nu * ood; + (*uvw)[1] = nv * ood; + (*uvw)[2] = 1.0f - (*uvw)[0] - (*uvw)[1]; +} + +void IKRay3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_heading"), &IKRay3D::get_heading); + ClassDB::bind_method(D_METHOD("get_scaled_projection", "input"), &IKRay3D::get_scaled_projection); + ClassDB::bind_method(D_METHOD("get_intersects_plane", "a", "b", "c"), &IKRay3D::get_intersects_plane); +} diff --git a/modules/many_bone_ik/src/ik_ray_3d.h b/modules/many_bone_ik/src/ik_ray_3d.h new file mode 100644 index 000000000000..4aa57a4058a8 --- /dev/null +++ b/modules/many_bone_ik/src/ik_ray_3d.h @@ -0,0 +1,128 @@ +/**************************************************************************/ +/* ik_ray_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef IK_RAY_3D_H +#define IK_RAY_3D_H + +#include "core/io/resource.h" +#include "core/math/vector3.h" + +class IKRay3D : public RefCounted { + GDCLASS(IKRay3D, RefCounted); + + Vector3 tta, ttb, ttc; + Vector3 I, u, v, n, dir, w0; + Vector3 m, at, bt, ct, pt; + Vector3 bc, ca, ac; + + Vector3 point_1; + Vector3 point_2; + Vector3 working_vector; + +protected: + static void _bind_methods(); + +public: + IKRay3D(); + ~IKRay3D() {} + IKRay3D(Vector3 p_point_one, Vector3 p_point_two); + Vector3 get_heading(); + void set_heading(const Vector3 &p_new_head); + + /** + * Returns the scalar projection of the input vector on this + * ray. In other words, if this ray goes from (5, 0) to (10, 0), + * and the input vector is (7.5, 7), this function + * would output 0.5. Because that is amount the ray would need + * to be scaled by so that its tip is where the vector would project onto + * this ray. + *

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

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

+ * 1. calling get_scaled_projection(someVector) should return the same value as + * calling + * get_scaled_projection(closestPointTo(someVector). + * 2. calling getMultipliedBy(get_scaled_projection(someVector)) should return the + * same + * vector as calling closestPointTo(someVector) + * + * @param p_input a vector to project onto this ray + */ + real_t get_scaled_projection(const Vector3 p_input); + + /** + * adds the specified length to the ray in both directions. + */ + void elongate(real_t p_amount); + + /** + * @param ta the first vertex of a triangle on the plane + * @param tb the second vertex of a triangle on the plane + * @param tc the third vertex of a triangle on the plane + * @return the point where this ray intersects the plane specified by the + * triangle ta,tb,tc. + */ + Vector3 get_intersects_plane(Vector3 p_vertex_a, Vector3 p_vertex_b, Vector3 p_vertex_c); + + /* + * Find where this ray intersects a sphere + * + * @param Vector3 the center of the sphere to test against. + * + * @param radius radius of the sphere + * + * @param S1 reference to variable in which the first intersection will be + * placed + * + * @param S2 reference to variable in which the second intersection will be + * placed + * + * @return number of intersections found; + */ + int intersects_sphere(Vector3 p_sphere_center, real_t p_radius, Vector3 *r_first_intersection, Vector3 *r_second_intersection); + void set_point_1(Vector3 p_point); + void set_point_2(Vector3 p_point); + Vector3 get_point_2(); + Vector3 get_point_1(); + int intersects_sphere(Vector3 p_rp1, Vector3 p_rp2, real_t p_radius, Vector3 *r_first_intersection, Vector3 *r_second_intersection); + real_t triangle_area_2d(real_t p_x1, real_t p_y1, real_t p_x2, real_t p_y2, real_t p_x3, real_t p_y3); + void barycentric(Vector3 p_a, Vector3 p_b, Vector3 p_c, Vector3 p_p, Vector3 *r_uvw); + Vector3 plane_intersect_test(Vector3 p_vertex_a, Vector3 p_vertex_b, Vector3 p_vertex_c, Vector3 *uvw); + operator String() const { + return String(L"(") + point_1.x + L" -> " + point_2.x + L") \n " + L"(" + point_1.y + L" -> " + point_2.y + L") \n " + L"(" + point_1.z + L" -> " + point_2.z + L") \n "; + } +}; + +#endif // IK_RAY_3D_H diff --git a/modules/many_bone_ik/src/many_bone_ik_3d.cpp b/modules/many_bone_ik/src/many_bone_ik_3d.cpp new file mode 100644 index 000000000000..36166020643f --- /dev/null +++ b/modules/many_bone_ik/src/many_bone_ik_3d.cpp @@ -0,0 +1,1097 @@ +/**************************************************************************/ +/* 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 ManyBoneIK3D::set_total_effector_count(int32_t p_value) { + int32_t old_count = pins.size(); + pin_count = p_value; + pins.resize(p_value); + for (int32_t pin_i = p_value; pin_i-- > old_count;) { + pins.write[pin_i].instantiate(); + } + set_dirty(); +} + +int32_t ManyBoneIK3D::get_effector_count() const { + return pin_count; +} + +void ManyBoneIK3D::set_effector_count(int32_t p_pin_count) { + pin_count = p_pin_count; +} + +void ManyBoneIK3D::set_effector_target_node_path(int32_t p_pin_index, const NodePath &p_target_node) { + ERR_FAIL_INDEX(p_pin_index, pins.size()); + Ref effector_template = pins[p_pin_index]; + if (effector_template.is_null()) { + effector_template.instantiate(); + pins.write[p_pin_index] = effector_template; + } + effector_template->set_target_node(p_target_node); + set_dirty(); +} + +NodePath ManyBoneIK3D::get_effector_target_node_path(int32_t p_pin_index) { + ERR_FAIL_INDEX_V(p_pin_index, pins.size(), NodePath()); + const Ref effector_template = pins[p_pin_index]; + return effector_template->get_target_node(); +} + +Vector> ManyBoneIK3D::_get_bone_effectors() const { + return pins; +} + +void ManyBoneIK3D::_remove_pin(int32_t p_index) { + ERR_FAIL_INDEX(p_index, pins.size()); + pins.remove_at(p_index); + pin_count--; + pins.resize(pin_count); + set_dirty(); +} + +void ManyBoneIK3D::_update_ik_bones_transform() { + for (int32_t bone_i = bone_list.size(); bone_i-- > 0;) { + Ref bone = bone_list[bone_i]; + if (bone.is_null()) { + continue; + } + bone->set_initial_pose(get_skeleton()); + if (bone->is_pinned()) { + bone->get_pin()->update_target_global_transform(get_skeleton(), this); + } + } +} + +void ManyBoneIK3D::_update_skeleton_bones_transform() { + for (int32_t bone_i = bone_list.size(); bone_i-- > 0;) { + Ref bone = bone_list[bone_i]; + if (bone.is_null()) { + continue; + } + if (bone->get_bone_id() == -1) { + continue; + } + bone->set_skeleton_bone_pose(get_skeleton()); + } + update_gizmos(); +} + +void ManyBoneIK3D::_get_property_list(List *p_list) const { + const Vector> ik_bones = get_bone_list(); + RBSet existing_pins; + for (int32_t pin_i = 0; pin_i < get_effector_count(); pin_i++) { + const String bone_name = get_effector_bone_name(pin_i); + existing_pins.insert(bone_name); + } + const uint32_t pin_usage = PROPERTY_USAGE_DEFAULT; + p_list->push_back( + PropertyInfo(Variant::INT, "pin_count", + PROPERTY_HINT_RANGE, "0,65536,or_greater", pin_usage | PROPERTY_USAGE_ARRAY | PROPERTY_USAGE_READ_ONLY, + "Pins,pins/")); + for (int pin_i = 0; pin_i < get_effector_count(); pin_i++) { + PropertyInfo effector_name; + effector_name.type = Variant::STRING_NAME; + effector_name.name = "pins/" + itos(pin_i) + "/bone_name"; + effector_name.usage = pin_usage; + if (get_skeleton()) { + String names; + for (int bone_i = 0; bone_i < get_skeleton()->get_bone_count(); bone_i++) { + String name = get_skeleton()->get_bone_name(bone_i); + StringName string_name = StringName(name); + if (existing_pins.has(string_name)) { + continue; + } + name += ","; + names += name; + existing_pins.insert(name); + } + effector_name.hint = PROPERTY_HINT_ENUM_SUGGESTION; + effector_name.hint_string = names; + } else { + effector_name.hint = PROPERTY_HINT_NONE; + effector_name.hint_string = ""; + } + p_list->push_back(effector_name); + p_list->push_back( + PropertyInfo(Variant::NODE_PATH, "pins/" + itos(pin_i) + "/target_node", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D", pin_usage)); + p_list->push_back( + PropertyInfo(Variant::BOOL, "pins/" + itos(pin_i) + "/target_static", PROPERTY_HINT_NONE, "", pin_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "pins/" + itos(pin_i) + "/motion_propagation_factor", PROPERTY_HINT_RANGE, "0,1,0.1,or_greater", pin_usage)); + p_list->push_back( + PropertyInfo(Variant::FLOAT, "pins/" + itos(pin_i) + "/weight", PROPERTY_HINT_RANGE, "0,1,0.1,or_greater", pin_usage)); + p_list->push_back( + PropertyInfo(Variant::VECTOR3, "pins/" + itos(pin_i) + "/direction_priorities", PROPERTY_HINT_RANGE, "0,1,0.1,or_greater", pin_usage)); + } + 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 == "constraint_count") { + r_ret = get_constraint_count(); + return true; + } else if (name == "pin_count") { + r_ret = get_effector_count(); + return true; + } else if (name == "bone_count") { + r_ret = get_bone_count(); + return true; + } else if (name.begins_with("pins/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + ERR_FAIL_INDEX_V(index, pins.size(), false); + Ref effector_template = pins[index]; + ERR_FAIL_NULL_V(effector_template, false); + if (what == "bone_name") { + r_ret = effector_template->get_name(); + return true; + } else if (what == "target_node") { + r_ret = effector_template->get_target_node(); + return true; + } else if (what == "target_static") { + r_ret = effector_template->get_target_node().is_empty(); + return true; + } else if (what == "motion_propagation_factor") { + r_ret = get_pin_motion_propagation_factor(index); + return true; + } else if (what == "weight") { + r_ret = get_pin_weight(index); + return true; + } else if (what == "direction_priorities") { + r_ret = get_pin_direction_priorities(index); + return true; + } + } else if (name.begins_with("constraints/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + ERR_FAIL_INDEX_V(index, constraint_count, false); + String begins = "constraints/" + itos(index) + "/kusudama_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 == "constraint_count") { + _set_constraint_count(p_value); + return true; + } else if (name == "pin_count") { + set_total_effector_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_total_effector_count(constraint_count); + } + if (what == "bone_name") { + set_effector_bone_name(index, p_value); + return true; + } else if (what == "target_node") { + set_effector_target_node_path(index, p_value); + return true; + } else if (what == "target_static") { + if (p_value) { + set_effector_target_node_path(index, NodePath()); + } + return true; + } else if (what == "motion_propagation_factor") { + set_pin_motion_propagation_factor(index, p_value); + return true; + } else if (what == "weight") { + set_pin_weight(index, p_value); + return true; + } else if (what == "direction_priorities") { + set_pin_direction_priorities(index, p_value); + return true; + } + } else if (name.begins_with("constraints/")) { + int index = name.get_slicec('/', 1).to_int(); + String what = name.get_slicec('/', 2); + String begins = "constraints/" + itos(index) + "/kusudama_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_total_effector_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_constraint_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_effector_count); + ClassDB::bind_method(D_METHOD("set_pin_count", "count"), &ManyBoneIK3D::set_effector_count); + + ClassDB::bind_method(D_METHOD("get_effector_bone_name", "index"), &ManyBoneIK3D::get_effector_bone_name); + ClassDB::bind_method(D_METHOD("get_pin_direction_priorities", "index"), &ManyBoneIK3D::get_pin_direction_priorities); + ClassDB::bind_method(D_METHOD("set_pin_direction_priorities", "index", "priority"), &ManyBoneIK3D::set_pin_direction_priorities); + ClassDB::bind_method(D_METHOD("get_effector_pin_node_path", "index"), &ManyBoneIK3D::get_effector_pin_node_path); + ClassDB::bind_method(D_METHOD("set_effector_pin_node_path", "index", "nodepath"), &ManyBoneIK3D::set_effector_pin_node_path); + 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_effector_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"); +} + +ManyBoneIK3D::ManyBoneIK3D() { +} + +ManyBoneIK3D::~ManyBoneIK3D() { +} + +float ManyBoneIK3D::get_pin_motion_propagation_factor(int32_t p_effector_index) const { + ERR_FAIL_INDEX_V(p_effector_index, pins.size(), 0.0f); + const Ref effector_template = pins[p_effector_index]; + return effector_template->get_motion_propagation_factor(); +} + +void ManyBoneIK3D::set_pin_motion_propagation_factor(int32_t p_effector_index, const float p_motion_propagation_factor) { + ERR_FAIL_INDEX(p_effector_index, pins.size()); + Ref effector_template = pins[p_effector_index]; + ERR_FAIL_NULL(effector_template); + effector_template->set_motion_propagation_factor(p_motion_propagation_factor); + set_dirty(); +} + +void ManyBoneIK3D::_set_constraint_count(int32_t p_count) { + int32_t old_count = constraint_names.size(); + constraint_count = p_count; + constraint_names.resize(p_count); + 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_effector_id(StringName p_bone_name) { + for (int32_t constraint_i = 0; constraint_i < constraint_count; constraint_i++) { + if (constraint_names[constraint_i] == p_bone_name) { + return constraint_i; + } + } + return -1; +} + +void ManyBoneIK3D::set_kusudama_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; +} + +void ManyBoneIK3D::set_default_damp(float p_default_damp) { + default_damp = p_default_damp; + set_dirty(); +} + +StringName ManyBoneIK3D::get_effector_bone_name(int32_t p_effector_index) const { + ERR_FAIL_INDEX_V(p_effector_index, pins.size(), ""); + Ref effector_template = pins[p_effector_index]; + return effector_template->get_name(); +} + +void ManyBoneIK3D::set_kusudama_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; +} + +float ManyBoneIK3D::get_iterations_per_frame() const { + return iterations_per_frame; +} + +void ManyBoneIK3D::set_iterations_per_frame(const float &p_iterations_per_frame) { + iterations_per_frame = p_iterations_per_frame; +} + +void ManyBoneIK3D::set_effector_pin_node_path(int32_t p_effector_index, NodePath p_node_path) { + ERR_FAIL_INDEX(p_effector_index, pins.size()); + Node *node = get_node_or_null(p_node_path); + if (!node) { + return; + } + Ref effector_template = pins[p_effector_index]; + effector_template->set_target_node(p_node_path); +} + +NodePath ManyBoneIK3D::get_effector_pin_node_path(int32_t p_effector_index) const { + ERR_FAIL_INDEX_V(p_effector_index, pins.size(), NodePath()); + Ref effector_template = pins[p_effector_index]; + return effector_template->get_target_node(); +} + +void ManyBoneIK3D::_process_modification() { + if (!get_skeleton()) { + return; + } + if (get_effector_count() == 0) { + 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(); + } + bool has_pins = false; + for (Ref pin : pins) { + if (pin.is_valid() && !pin->get_name().is_empty()) { + has_pins = true; + break; + } + } + if (!has_pins) { + return; + } + 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(); +} + +real_t ManyBoneIK3D::get_pin_weight(int32_t p_pin_index) const { + ERR_FAIL_INDEX_V(p_pin_index, pins.size(), 0.0); + const Ref effector_template = pins[p_pin_index]; + return effector_template->get_weight(); +} + +void ManyBoneIK3D::set_pin_weight(int32_t p_pin_index, const real_t &p_weight) { + ERR_FAIL_INDEX(p_pin_index, pins.size()); + Ref effector_template = pins[p_pin_index]; + if (effector_template.is_null()) { + effector_template.instantiate(); + pins.write[p_pin_index] = effector_template; + } + effector_template->set_weight(p_weight); + set_dirty(); +} + +Vector3 ManyBoneIK3D::get_pin_direction_priorities(int32_t p_pin_index) const { + ERR_FAIL_INDEX_V(p_pin_index, pins.size(), Vector3(0, 0, 0)); + const Ref effector_template = pins[p_pin_index]; + return effector_template->get_direction_priorities(); +} + +void ManyBoneIK3D::set_pin_direction_priorities(int32_t p_pin_index, const Vector3 &p_priority_direction) { + ERR_FAIL_INDEX(p_pin_index, pins.size()); + Ref effector_template = pins[p_pin_index]; + if (effector_template.is_null()) { + effector_template.instantiate(); + pins.write[p_pin_index] = effector_template; + } + effector_template->set_direction_priorities(p_priority_direction); + set_dirty(); +} + +void ManyBoneIK3D::set_dirty() { + is_dirty = true; +} + +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_constraint_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;) { + bone_damp.write[bone_i] = get_default_damp(); + } + bone_count = p_count; + set_dirty(); + notify_property_list_changed(); +} + +int32_t ManyBoneIK3D::get_bone_count() const { + return bone_count; +} + +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]; + if (effector_template->get_target_node().is_empty()) { + return true; + } + return !effector_template->get_target_node().is_empty(); +} + +void ManyBoneIK3D::register_skeleton() { + if (!get_effector_count() && !get_constraint_count()) { + reset_constraints(); + } + set_dirty(); +} + +void ManyBoneIK3D::reset_constraints() { + Skeleton3D *skeleton = get_skeleton(); + if (skeleton) { + int32_t saved_pin_count = get_effector_count(); + set_total_effector_count(0); + set_total_effector_count(saved_pin_count); + int32_t saved_constraint_count = constraint_names.size(); + _set_constraint_count(0); + _set_constraint_count(saved_constraint_count); + _set_bone_count(0); + _set_bone_count(saved_constraint_count); + } + set_dirty(); +} + +bool ManyBoneIK3D::get_constraint_mode() const { + return is_constraint_mode; +} + +void ManyBoneIK3D::set_constraint_mode(bool p_enabled) { + is_constraint_mode = p_enabled; +} + +int32_t ManyBoneIK3D::get_ui_selected_bone() const { + return ui_selected_bone; +} + +void ManyBoneIK3D::set_ui_selected_bone(int32_t p_ui_selected_bone) { + ui_selected_bone = p_ui_selected_bone; +} + +void ManyBoneIK3D::set_stabilization_passes(int32_t p_passes) { + stabilize_passes = p_passes; + set_dirty(); +} + +int32_t ManyBoneIK3D::get_stabilization_passes() { + return stabilize_passes; +} + +Transform3D ManyBoneIK3D::get_godot_skeleton_transform_inverse() { + return godot_skeleton_transform_inverse; +} + +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_effector_bone_name(pin_i) == p_string) { + return pin_i; + } + } + return -1; +} + +bool ManyBoneIK3D::get_effector_target_fixed(int32_t p_effector_index) { + ERR_FAIL_INDEX_V(p_effector_index, pins.size(), false); + Ref effector_template = pins[p_effector_index]; + return get_effector_pin_node_path(p_effector_index).is_empty(); +} + +void ManyBoneIK3D::set_effector_target_fixed(int32_t p_effector_index, bool p_force_ignore) { + ERR_FAIL_INDEX(p_effector_index, pins.size()); + if (!p_force_ignore) { + return; + } + Ref effector_template = pins[p_effector_index]; + effector_template->set_target_node(NodePath()); + set_dirty(); +} + +void ManyBoneIK3D::_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); + Ref segmented_skeleton = Ref(memnew(IKBoneSegment3D(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 ManyBoneIK3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) { + if (p_old) { + if (p_old->is_connected(SNAME("bone_list_changed"), callable_mp(this, &ManyBoneIK3D::_bone_list_changed))) { + p_old->disconnect(SNAME("bone_list_changed"), callable_mp(this, &ManyBoneIK3D::_bone_list_changed)); + } + } + if (p_new) { + if (!p_new->is_connected(SNAME("bone_list_changed"), callable_mp(this, &ManyBoneIK3D::_bone_list_changed))) { + p_new->connect(SNAME("bone_list_changed"), callable_mp(this, &ManyBoneIK3D::_bone_list_changed)); + } + } + if (is_connected(SNAME("modification_processed"), callable_mp(this, &ManyBoneIK3D::_update_ik_bones_transform))) { + disconnect(SNAME("modification_processed"), callable_mp(this, &ManyBoneIK3D::_update_ik_bones_transform)); + } + connect(SNAME("modification_processed"), callable_mp(this, &ManyBoneIK3D::_update_ik_bones_transform)); + _bone_list_changed(); +} + +void ManyBoneIK3D::set_effector_bone_name(int32_t p_pin_index, const String &p_bone) { + ERR_FAIL_INDEX(p_pin_index, pins.size()); + Ref effector_template = pins[p_pin_index]; + if (effector_template.is_null()) { + effector_template.instantiate(); + pins.write[p_pin_index] = effector_template; + } + effector_template->set_name(p_bone); + set_dirty(); +} diff --git a/modules/many_bone_ik/src/many_bone_ik_3d.h b/modules/many_bone_ik/src/many_bone_ik_3d.h new file mode 100644 index 000000000000..f091dc740a9d --- /dev/null +++ b/modules/many_bone_ik/src/many_bone_ik_3d.h @@ -0,0 +1,160 @@ +/**************************************************************************/ +/* 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" +#include "scene/main/scene_tree.h" + +class ManyBoneIK3DState; +class ManyBoneIK3D : public SkeletonModifier3D { + GDCLASS(ManyBoneIK3D, 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> 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; + 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(); + Vector> _get_bone_effectors() const; + void set_constraint_name_at_index(int32_t p_index, String p_name); + void set_total_effector_count(int32_t p_value); + void _set_constraint_count(int32_t p_count); + void _remove_pin(int32_t p_index); + void _set_bone_count(int32_t p_count); + 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_effector_target_fixed(int32_t p_effector_index, bool p_force_ignore); + bool get_effector_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; + void set_iterations_per_frame(const float &p_iterations_per_frame); + void queue_print_skeleton(); + int32_t get_effector_count() const; + void set_effector_count(int32_t p_pin_count); + void remove_constraint_at_index(int32_t p_index); + void set_effector_bone_name(int32_t p_pin_index, const String &p_bone); + StringName get_effector_bone_name(int32_t p_effector_index) const; + void set_effector_pin_node_path(int32_t p_effector_index, NodePath p_node_path); + NodePath get_effector_pin_node_path(int32_t p_effector_index) const; + int32_t find_effector_id(StringName p_bone_name); + void set_effector_target_node_path(int32_t p_effector_index, const NodePath &p_target_node); + void set_pin_weight(int32_t p_pin_index, const real_t &p_weight); + real_t get_pin_weight(int32_t p_pin_index) const; + void set_pin_direction_priorities(int32_t p_pin_index, const Vector3 &p_priority_direction); + Vector3 get_pin_direction_priorities(int32_t p_pin_index) const; + NodePath get_effector_target_node_path(int32_t p_pin_index); + void set_pin_motion_propagation_factor(int32_t p_effector_index, const float p_motion_propagation_factor); + 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(); +}; + +#endif // MANY_BONE_IK_3D_H diff --git a/modules/many_bone_ik/src/math/ik_node_3d.cpp b/modules/many_bone_ik/src/math/ik_node_3d.cpp new file mode 100644 index 000000000000..61516bdd2f65 --- /dev/null +++ b/modules/many_bone_ik/src/math/ik_node_3d.cpp @@ -0,0 +1,159 @@ +/**************************************************************************/ +/* ik_node_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "ik_node_3d.h" + +void IKNode3D::_propagate_transform_changed() { + Vector> to_remove; + + for (Ref transform : children) { + if (transform.is_null()) { + to_remove.push_back(transform); + } else { + transform->_propagate_transform_changed(); + } + } + + for (Ref transform : to_remove) { + children.erase(transform); + } + + dirty |= DIRTY_GLOBAL; +} + +void IKNode3D::_update_local_transform() const { + local_transform.basis = rotation.scaled(scale); + dirty &= ~DIRTY_LOCAL; +} + +void IKNode3D::rotate_local_with_global(const Basis &p_basis, bool p_propagate) { + if (parent.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 IKNode3D::set_transform(const Transform3D &p_transform) { + if (local_transform != p_transform) { + local_transform = p_transform; + dirty |= DIRTY_VECTORS; + _propagate_transform_changed(); + } +} + +void IKNode3D::set_global_transform(const Transform3D &p_transform) { + 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 IKNode3D::get_transform() const { + if (dirty & DIRTY_LOCAL) { + _update_local_transform(); + } + + return local_transform; +} + +Transform3D IKNode3D::get_global_transform() const { + if (dirty & DIRTY_GLOBAL) { + if (dirty & DIRTY_LOCAL) { + _update_local_transform(); + } + 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 IKNode3D::set_disable_scale(bool p_enabled) { + disable_scale = p_enabled; +} + +bool IKNode3D::is_scale_disabled() const { + return disable_scale; +} + +void IKNode3D::set_parent(Ref p_parent) { + if (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 IKNode3D::get_parent() const { + return parent.get_ref(); +} + +Vector3 IKNode3D::to_local(const Vector3 &p_global) const { + return get_global_transform().affine_inverse().xform(p_global); +} + +Vector3 IKNode3D::to_global(const Vector3 &p_local) const { + return get_global_transform().xform(p_local); +} + +IKNode3D::~IKNode3D() { + cleanup(); +} + +void IKNode3D::_notification(int p_what) { + if (p_what == NOTIFICATION_PREDELETE) { + cleanup(); + } +} +void IKNode3D::cleanup() { + for (Ref &child : children) { + child->set_parent(Ref()); + } +} diff --git a/modules/many_bone_ik/src/math/ik_node_3d.h b/modules/many_bone_ik/src/math/ik_node_3d.h new file mode 100644 index 000000000000..b84af288e9b3 --- /dev/null +++ b/modules/many_bone_ik/src/math/ik_node_3d.h @@ -0,0 +1,102 @@ +/**************************************************************************/ +/* ik_node_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef IK_NODE_3D_H +#define IK_NODE_3D_H + +#include "core/object/ref_counted.h" +#include "core/templates/list.h" + +#include "core/io/resource.h" +#include "core/math/transform_3d.h" + +class IKNode3D : public RefCounted { + GDCLASS(IKNode3D, RefCounted); + + enum TransformDirty { + DIRTY_NONE = 0, + DIRTY_VECTORS = 1, + DIRTY_LOCAL = 2, + DIRTY_GLOBAL = 4 + }; + + mutable Transform3D global_transform; + mutable Transform3D local_transform; + mutable Basis rotation; + mutable Vector3 scale = Vector3(1, 1, 1); + + mutable int dirty = DIRTY_NONE; + + 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"), &IKNode3D::_propagate_transform_changed); + ClassDB::bind_method(D_METHOD("_update_local_transform"), &IKNode3D::_update_local_transform); + ClassDB::bind_method(D_METHOD("rotate_local_with_global", "p_basis", "p_propagate"), &IKNode3D::rotate_local_with_global, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("set_transform", "p_transform"), &IKNode3D::set_transform); + ClassDB::bind_method(D_METHOD("set_global_transform", "p_transform"), &IKNode3D::set_global_transform); + ClassDB::bind_method(D_METHOD("get_transform"), &IKNode3D::get_transform); + ClassDB::bind_method(D_METHOD("get_global_transform"), &IKNode3D::get_global_transform); + ClassDB::bind_method(D_METHOD("set_disable_scale", "p_enabled"), &IKNode3D::set_disable_scale); + ClassDB::bind_method(D_METHOD("is_scale_disabled"), &IKNode3D::is_scale_disabled); + ClassDB::bind_method(D_METHOD("set_parent", "p_parent"), &IKNode3D::set_parent); + ClassDB::bind_method(D_METHOD("get_parent"), &IKNode3D::get_parent); + ClassDB::bind_method(D_METHOD("to_local", "p_global"), &IKNode3D::to_local); + ClassDB::bind_method(D_METHOD("to_global", "p_local"), &IKNode3D::to_global); + } + +public: + void _propagate_transform_changed(); + void set_transform(const Transform3D &p_transform); + void set_global_transform(const Transform3D &p_transform); + Transform3D get_transform() const; + Transform3D get_global_transform() const; + + void set_disable_scale(bool p_enabled); + bool is_scale_disabled() const; + + void set_parent(Ref p_parent); + Ref get_parent() const; + + Vector3 to_local(const Vector3 &p_global) const; + Vector3 to_global(const Vector3 &p_local) const; + void rotate_local_with_global(const Basis &p_basis, bool p_propagate = false); + void cleanup(); + ~IKNode3D(); +}; + +#endif // IK_NODE_3D_H diff --git a/modules/many_bone_ik/src/math/qcp.cpp b/modules/many_bone_ik/src/math/qcp.cpp new file mode 100644 index 000000000000..221a6f66d357 --- /dev/null +++ b/modules/many_bone_ik/src/math/qcp.cpp @@ -0,0 +1,248 @@ +/**************************************************************************/ +/* qcp.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "qcp.h" + +QCP::QCP(double p_evec_prec) { + eigenvector_precision = p_evec_prec; +} + +void QCP::set(PackedVector3Array &r_target, PackedVector3Array &r_moved) { + target = r_target; + moved = r_moved; + transformation_calculated = false; + inner_product_calculated = false; +} + +Quaternion QCP::get_rotation() { + Quaternion result; + if (!transformation_calculated) { + if (!inner_product_calculated) { + inner_product(target, moved); + } + result = calculate_rotation(); + transformation_calculated = true; + } + return result; +} + +Quaternion QCP::calculate_rotation() { + Quaternion result; + + if (moved.size() == 1) { + Vector3 u = moved[0]; + Vector3 v = target[0]; + double norm_product = u.length() * v.length(); + + if (norm_product == 0.0) { + return Quaternion(); + } + + double dot = u.dot(v); + + if (dot < ((2.0e-15 - 1.0) * norm_product)) { + Vector3 w = u.normalized(); + result = Quaternion(w.x, w.y, w.z, 0.0f).normalized(); + } else { + double q0 = Math::sqrt(0.5 * (1.0 + dot / norm_product)); + double coeff = 1.0 / (2.0 * q0 * norm_product); + Vector3 q = v.cross(u).normalized(); + result = Quaternion(coeff * q.x, coeff * q.y, coeff * q.z, q0).normalized(); + } + } else { + double a13 = -sum_xz_minus_zx; + double a14 = sum_xy_minus_yx; + double a21 = sum_yz_minus_zy; + double a22 = sum_xx_minus_yy - sum_zz - max_eigenvalue; + double a23 = sum_xy_plus_yx; + double a24 = sum_xz_plus_zx; + double a31 = a13; + double a32 = a23; + double a33 = sum_yy - sum_xx - sum_zz - max_eigenvalue; + double a34 = sum_yz_plus_zy; + double a41 = a14; + double a42 = a24; + double a43 = a34; + double a44 = sum_zz - sum_xx_plus_yy - max_eigenvalue; + + double a3344_4334 = a33 * a44 - a43 * a34; + double a3244_4234 = a32 * a44 - a42 * a34; + double a3243_4233 = a32 * a43 - a42 * a33; + double a3143_4133 = a31 * a43 - a41 * a33; + double a3144_4134 = a31 * a44 - a41 * a34; + double a3142_4132 = a31 * a42 - a41 * a32; + + double quaternion_w = a22 * a3344_4334 - a23 * a3244_4234 + a24 * a3243_4233; + double quaternion_x = -a21 * a3344_4334 + a23 * a3144_4134 - a24 * a3143_4133; + double quaternion_y = a21 * a3244_4234 - a22 * a3144_4134 + a24 * a3142_4132; + double quaternion_z = -a21 * a3243_4233 + a22 * a3143_4133 - a23 * a3142_4132; + double qsqr = quaternion_w * quaternion_w + quaternion_x * quaternion_x + quaternion_y * quaternion_y + quaternion_z * quaternion_z; + + if (qsqr < eigenvector_precision) { + 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 QCP::translate(Vector3 r_translate, PackedVector3Array &r_x) { + for (Vector3 &p : r_x) { + p += r_translate; + } +} + +Vector3 QCP::get_translation() { + return target_center - moved_center; +} + +Vector3 QCP::move_to_weighted_center(PackedVector3Array &r_to_center, Vector &r_weight) { + Vector3 center; + double total_weight = 0; + bool weight_is_empty = r_weight.is_empty(); + int size = r_to_center.size(); + + for (int i = 0; i < size; i++) { + if (!weight_is_empty) { + total_weight += r_weight[i]; + center += r_to_center[i] * r_weight[i]; + } else { + center += r_to_center[i]; + total_weight++; + } + } + + if (total_weight > 0) { + center /= total_weight; + } + + return center; +} + +void QCP::inner_product(PackedVector3Array &coords1, PackedVector3Array &coords2) { + Vector3 weighted_coord1, weighted_coord2; + double sum_of_squares1 = 0, sum_of_squares2 = 0; + + sum_xx = 0; + sum_xy = 0; + sum_xz = 0; + sum_yx = 0; + sum_yy = 0; + sum_yz = 0; + sum_zx = 0; + sum_zy = 0; + sum_zz = 0; + + bool weight_is_empty = weight.is_empty(); + int size = coords1.size(); + + for (int i = 0; i < size; i++) { + if (!weight_is_empty) { + weighted_coord1 = weight[i] * coords1[i]; + sum_of_squares1 += weighted_coord1.dot(coords1[i]); + } else { + weighted_coord1 = coords1[i]; + sum_of_squares1 += weighted_coord1.dot(weighted_coord1); + } + + weighted_coord2 = coords2[i]; + + sum_of_squares2 += weight_is_empty ? weighted_coord2.dot(weighted_coord2) : (weight[i] * weighted_coord2.dot(weighted_coord2)); + + sum_xx += (weighted_coord1.x * weighted_coord2.x); + sum_xy += (weighted_coord1.x * weighted_coord2.y); + sum_xz += (weighted_coord1.x * weighted_coord2.z); + + sum_yx += (weighted_coord1.y * weighted_coord2.x); + sum_yy += (weighted_coord1.y * weighted_coord2.y); + sum_yz += (weighted_coord1.y * weighted_coord2.z); + + sum_zx += (weighted_coord1.z * weighted_coord2.x); + sum_zy += (weighted_coord1.z * weighted_coord2.y); + sum_zz += (weighted_coord1.z * weighted_coord2.z); + } + + double initial_eigenvalue = (sum_of_squares1 + sum_of_squares2) * 0.5; + + sum_xz_plus_zx = sum_xz + sum_zx; + sum_yz_plus_zy = sum_yz + sum_zy; + sum_xy_plus_yx = sum_xy + sum_yx; + sum_yz_minus_zy = sum_yz - sum_zy; + sum_xz_minus_zx = sum_xz - sum_zx; + sum_xy_minus_yx = sum_xy - sum_yx; + sum_xx_plus_yy = sum_xx + sum_yy; + sum_xx_minus_yy = sum_xx - sum_yy; + max_eigenvalue = initial_eigenvalue; + + inner_product_calculated = true; +} + +Quaternion QCP::weighted_superpose(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool translate) { + set(p_moved, p_target, p_weight, translate); + return get_rotation(); +} + +void QCP::set(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool p_translate) { + transformation_calculated = false; + inner_product_calculated = false; + + moved = p_moved; + target = p_target; + weight = p_weight; + + if (p_translate) { + moved_center = move_to_weighted_center(moved, weight); + w_sum = 0; // set wsum to 0 so we don't double up. + target_center = move_to_weighted_center(target, weight); + translate(moved_center * -1, moved); + translate(target_center * -1, target); + } else { + if (!p_weight.is_empty()) { + for (int i = 0; i < p_weight.size(); i++) { + w_sum += p_weight[i]; + } + } else { + w_sum = p_moved.size(); + } + } +} diff --git a/modules/many_bone_ik/src/math/qcp.h b/modules/many_bone_ik/src/math/qcp.h new file mode 100644 index 000000000000..4b42c554eec0 --- /dev/null +++ b/modules/many_bone_ik/src/math/qcp.h @@ -0,0 +1,99 @@ +/**************************************************************************/ +/* qcp.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef QCP_H +#define QCP_H + +#include "core/math/basis.h" +#include "core/math/vector3.h" +#include "core/variant/variant.h" + +/** + * Implementation of the Quaternion-Based Characteristic Polynomial algorithm + * for RMSD and Superposition calculations. + * + * Usage: + * 1. Create a QCP object with two Vector3 arrays of equal length as input. + * The input coordinates are not changed. + * 2. Optionally, provide weighting factors [0 - 1] for each point. + * 3. For maximum efficiency, create a QCP object once and reuse it. + * + * A. Calculate rmsd only: double rmsd = qcp.getRmsd(); + * B. Calculate a 4x4 transformation (Quaternion and translation) matrix: Matrix4f trans = qcp.getTransformationMatrix(); + * C. Get transformed points (y superposed onto the reference x): Vector3[] ySuperposed = qcp.getTransformedCoordinates(); + * + * Citations: + * - Liu P, Agrafiotis DK, & Theobald DL (2011) Reply to comment on: "Fast determination of the optimal Quaternionation matrix for macromolecular superpositions." Journal of Computational Chemistry 32(1):185-186. [http://dx.doi.org/10.1002/jcc.21606] + * - Liu P, Agrafiotis DK, & Theobald DL (2010) "Fast determination of the optimal Quaternionation matrix for macromolecular superpositions." Journal of Computational Chemistry 31(7):1561-1563. [http://dx.doi.org/10.1002/jcc.21439] + * - Douglas L Theobald (2005) "Rapid calculation of RMSDs using a quaternion-based characteristic polynomial." Acta Crystallogr A 61(4):478-480. [http://dx.doi.org/10.1107/S0108767305015266] + * + * This is an adaptation of the original C code QCPQuaternion 1.4 (2012, October 10) to C++. + * The original C source code is available from http://theobald.brandeis.edu/qcp/ and was developed by: + * - Douglas L. Theobald, Department of Biochemistry, Brandeis University + * - Pu Liu, Johnson & Johnson Pharmaceutical Research and Development, L.L.C. + * + * @author Douglas L. Theobald (original C code) + * @author Pu Liu (original C code) + * @author Peter Rose (adapted to Java) + * @author Aleix Lafita (adapted to Java) + * @author Eron Gjoni (adapted to EWB IK) + * @author K. S. Ernest (iFire) Lee (adapted to ManyBoneIK) + */ + +class QCP { + double eigenvector_precision = 1E-6; + + 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); + +public: + QCP(double p_evec_prec); + Quaternion weighted_superpose(PackedVector3Array &p_moved, PackedVector3Array &p_target, Vector &p_weight, bool translate); + Quaternion get_rotation(); + Vector3 get_translation(); +}; + +#endif // QCP_H diff --git a/modules/many_bone_ik/tests/test_ik_kusudama_3d.h b/modules/many_bone_ik/tests/test_ik_kusudama_3d.h new file mode 100644 index 000000000000..6f16190bf59b --- /dev/null +++ b/modules/many_bone_ik/tests/test_ik_kusudama_3d.h @@ -0,0 +1,296 @@ +/**************************************************************************/ +/* test_ik_kusudama_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef TEST_IK_KUSUDAMA_3D_H +#define TEST_IK_KUSUDAMA_3D_H +#include "modules/many_bone_ik/src/ik_kusudama_3d.h" +#include "tests/test_macros.h" + +namespace TestIKKusudama3D { + +TEST_CASE("[Modules][ManyBoneIK][IKKusudama3D] Test a point inside or on the bounds with radius 30 degrees") { + Ref kusudama; + kusudama.instantiate(); + + Vector3 limit_cone_control_point = Vector3(0, 0, 1); + real_t limit_cone_radius = Math_PI / 6; // 30 degrees + + Ref cone; + cone.instantiate(); + cone->set_attached_to(kusudama); + cone->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone->set_radius(MAX(1.0e-38, limit_cone_radius)); + cone->set_control_point(limit_cone_control_point.normalized()); + + kusudama->add_open_cone(cone); + + TypedArray open_cones = kusudama->get_open_cones(); + REQUIRE(open_cones.size() == 1); + + Vector bounds; + bounds.resize(2); + bounds.write[0] = 0; + bounds.write[1] = 0; + Vector3 returned_point_outside = kusudama->get_local_point_in_limits(limit_cone_control_point, &bounds); + CHECK(bounds[0] > 0); + CHECK(returned_point_outside == limit_cone_control_point); +} + +TEST_CASE("[Modules][ManyBoneIK][IKKusudama3D] Test a point inside or on the bounds with radius 0 degrees") { + Ref kusudama; + kusudama.instantiate(); + + Vector3 limit_cone_control_point = Vector3(0, 0, 1); + real_t limit_cone_radius = 0; + + Ref cone; + cone.instantiate(); + cone->set_attached_to(kusudama); + cone->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone->set_radius(MAX(1.0e-38, limit_cone_radius)); + cone->set_control_point(limit_cone_control_point.normalized()); + + kusudama->add_open_cone(cone); + + TypedArray open_cones = kusudama->get_open_cones(); + CHECK_EQ(open_cones.size(), 1); + + Vector bounds; + bounds.resize(2); + bounds.write[0] = 0; + bounds.write[1] = 0; + Vector3 returned_point_outside = kusudama->get_local_point_in_limits(limit_cone_control_point, &bounds); + CHECK_LT(bounds[0], 0); + CHECK(returned_point_outside.is_equal_approx(limit_cone_control_point)); +} + +TEST_CASE("[Modules][ManyBoneIK][IKKusudama3D] Test a point outside the bounds with radius 0 degrees") { + Ref kusudama; + kusudama.instantiate(); + + Vector3 limit_cone_control_point = Vector3(0, 0, 1); + real_t limit_cone_radius = 0; + + Ref cone; + cone.instantiate(); + cone->set_attached_to(kusudama); + cone->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone->set_radius(MAX(1.0e-38, limit_cone_radius)); + cone->set_control_point(limit_cone_control_point.normalized()); + + kusudama->add_open_cone(cone); + + TypedArray open_cones = kusudama->get_open_cones(); + REQUIRE(open_cones.size() == 1); + + Vector bounds; + bounds.resize(2); + bounds.write[0] = 0; + bounds.write[1] = 0; + + Vector3 test_point_outside = Vector3(1, 0, 0); + Vector3 returned_point_outside = kusudama->get_local_point_in_limits(test_point_outside, &bounds); + CHECK_EQ(bounds[0], -1); + CHECK(returned_point_outside.is_equal_approx(limit_cone_control_point)); +} + +TEST_CASE("[Modules][ManyBoneIK][IKKusudama3D] Test a point outside the bounds with radius 30 degrees") { + Ref kusudama; + kusudama.instantiate(); + + Vector3 limit_cone_control_point = Vector3(0, 0, 1); + real_t limit_cone_radius = Math::deg_to_rad(30.0f); + + Ref cone; + cone.instantiate(); + cone->set_attached_to(kusudama); + cone->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone->set_radius(MAX(1.0e-38, limit_cone_radius)); + cone->set_control_point(limit_cone_control_point.normalized()); + + kusudama->add_open_cone(cone); + + TypedArray open_cones = kusudama->get_open_cones(); + REQUIRE(open_cones.size() == 1); + + Vector bounds; + bounds.resize(2); + bounds.write[0] = 0; + bounds.write[1] = 0; + + Vector3 test_point_outside = Vector3(1, 0, 0); + Vector3 returned_point_outside = kusudama->get_local_point_in_limits(test_point_outside, &bounds); + CHECK_EQ(bounds[0], -1); + CHECK(returned_point_outside.is_equal_approx(Vector3(0.50000001261839133, 0, 0.86602539649920684))); +} + +TEST_CASE("[Modules][ManyBoneIK][IKKusudama3D] Adding and retrieving Limit Cones") { + Ref kusudama; + kusudama.instantiate(); + + Vector3 point_on_sphere(1, 0, 0); // Unit sphere point + double radius = Math_PI / 4; // 45 degrees + + Ref cone; + cone.instantiate(); + cone->set_attached_to(kusudama); + cone->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone->set_radius(MAX(1.0e-38, radius)); + cone->set_control_point(point_on_sphere.normalized()); + + kusudama->add_open_cone(cone); + + TypedArray open_cones = kusudama->get_open_cones(); + CHECK(open_cones.size() == 1); // Expect one limit cone + + Ref retrieved_cone = open_cones[0]; + CHECK(retrieved_cone.is_valid()); // Validate retrieved cone + CHECK(Math::is_equal_approx(retrieved_cone->get_radius(), radius)); // Radius check + CHECK(retrieved_cone->get_closest_path_point(Ref(), point_on_sphere) == point_on_sphere); + CHECK(retrieved_cone->get_closest_path_point(retrieved_cone, point_on_sphere) == point_on_sphere); // Check match + + Vector3 different_point_on_sphere(-1, 0, 0); // Opposite sphere point + + Ref cone_2; + cone_2.instantiate(); + cone_2->set_attached_to(kusudama); + cone_2->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone_2->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone_2->set_radius(MAX(1.0e-38, radius)); + cone_2->set_control_point(different_point_on_sphere.normalized()); + kusudama->add_open_cone(cone_2); + + open_cones = kusudama->get_open_cones(); + CHECK(open_cones.size() == 2); // Now expect two cones + + Ref second_retrieved_cone = open_cones[1]; + CHECK(second_retrieved_cone.is_valid()); // Validate second cone + CHECK(Math::is_equal_approx(second_retrieved_cone->get_radius(), radius)); // Radius check + CHECK(second_retrieved_cone->get_closest_path_point(Ref(), different_point_on_sphere) == different_point_on_sphere); +} + +TEST_CASE("[Modules][ManyBoneIK][IKKusudama3D] Verify limit cone removal") { + Ref kusudama; + kusudama.instantiate(); + + // Add a couple of limit cones + Vector3 first_control_point = Vector3(1, 0, 0); + real_t first_radius = Math_PI / 4; // 45 degrees + + Ref cone_3; + cone_3.instantiate(); + cone_3->set_attached_to(kusudama); + cone_3->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone_3->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone_3->set_radius(MAX(1.0e-38, first_radius)); + cone_3->set_control_point(first_control_point.normalized()); + + kusudama->add_open_cone(cone_3); + + Vector3 second_control_point = Vector3(0, 1, 0); + real_t second_radius = Math_PI / 6; // 30 degrees + + Ref cone_4; + cone_4.instantiate(); + cone_4->set_attached_to(kusudama); + cone_4->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone_4->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone_4->set_radius(MAX(1.0e-38, second_radius)); + cone_4->set_control_point(second_control_point.normalized()); + + kusudama->add_open_cone(cone_4); + + // Initial checks (expected two limit cones) + TypedArray open_cones = kusudama->get_open_cones(); + REQUIRE(open_cones.size() == 2); + + // Re-check limit cones + open_cones = kusudama->get_open_cones(); + + // Remove the first limit cone + kusudama->remove_open_cone(open_cones[0]); + + // Re-check limit cones + open_cones = kusudama->get_open_cones(); + CHECK(open_cones.size() == 1); // Only one limit cone should be left + Ref open_cone = open_cones[0]; + CHECK(open_cone->get_control_point() == second_control_point); // Ensure the remaining cone is the correct one +} + +TEST_CASE("[Modules][ManyBoneIK][IKKusudama3D] Check limit cones clear functionality") { + Ref kusudama; + kusudama.instantiate(); + + Ref cone_5; + cone_5.instantiate(); + cone_5->set_attached_to(kusudama); + cone_5->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone_5->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone_5->set_radius(MAX(1.0e-38, Math_PI / 4)); + cone_5->set_control_point(Vector3(1, 0, 0).normalized()); + + kusudama->add_open_cone(cone_5); // 45 degrees + + Ref cone_6; + cone_6.instantiate(); + cone_6->set_attached_to(kusudama); + cone_6->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone_6->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone_6->set_radius(MAX(1.0e-38, Math_PI / 6)); + cone_6->set_control_point(Vector3(0, 1, 0).normalized()); + kusudama->add_open_cone(cone_6); // 30 degrees + + Ref cone_7; + cone_7.instantiate(); + cone_7->set_attached_to(kusudama); + cone_7->set_tangent_circle_center_next_1(Vector3(0.0f, -1.0f, 0.0f)); + cone_7->set_tangent_circle_center_next_2(Vector3(0.0f, 1.0f, 0.0f)); + cone_7->set_radius(MAX(1.0e-38, Math_PI / 3)); + cone_7->set_control_point(Vector3(0, 1, 0).normalized()); + kusudama->add_open_cone(cone_7); // 60 degrees + + // Initial checks (three limit cones expected) + TypedArray open_cones = kusudama->get_open_cones(); + REQUIRE(open_cones.size() == 3); + + kusudama->clear_open_cones(); + + // Re-check limit cones - there should be none + open_cones = kusudama->get_open_cones(); + CHECK(open_cones.size() == 0); // Expect no limit cones to remain +} +} // namespace TestIKKusudama3D + +#endif // TEST_IK_KUSUDAMA_3D_H diff --git a/modules/many_bone_ik/tests/test_ik_node_3d.h b/modules/many_bone_ik/tests/test_ik_node_3d.h new file mode 100644 index 000000000000..02191429d238 --- /dev/null +++ b/modules/many_bone_ik/tests/test_ik_node_3d.h @@ -0,0 +1,109 @@ +/**************************************************************************/ +/* 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 "modules/many_bone_ik/src/math/ik_node_3d.h" +#include "tests/test_macros.h" + +namespace TestIKNode3D { + +TEST_CASE("[Modules][IKNode3D] 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][IKNode3D] 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][IKNode3D] 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][IKNode3D] 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][IKNode3D] 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 TestIKNode3D + +#endif // TEST_IK_NODE_3D_H diff --git a/modules/many_bone_ik/tests/test_qcp.h b/modules/many_bone_ik/tests/test_qcp.h new file mode 100644 index 000000000000..299b2f3d1c50 --- /dev/null +++ b/modules/many_bone_ik/tests/test_qcp.h @@ -0,0 +1,116 @@ +/**************************************************************************/ +/* 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/many_bone_ik/src/math/qcp.h" +#include "tests/test_macros.h" + +namespace TestQCP { + +TEST_CASE("[Modules][QCP] Weighted Superpose") { + double epsilon = CMP_EPSILON; + QCP qcp(epsilon); + + 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 + + Quaternion result = qcp.weighted_superpose(moved, target, weight, false); + CHECK(abs(result.x - expected.x) < epsilon); + CHECK(abs(result.y - expected.y) < epsilon); + CHECK(abs(result.z - expected.z) < epsilon); + CHECK(abs(result.w - expected.w) < epsilon); +} + +TEST_CASE("[Modules][QCP] Weighted Translation") { + double epsilon = CMP_EPSILON; + QCP qcp(epsilon); + + Quaternion expected; + PackedVector3Array moved = { Vector3(4, 5, 6), Vector3(7, 8, 9), Vector3(1, 2, 3) }; + PackedVector3Array target = moved; + Vector3 translation_vector = Vector3(1, 2, 3); + for (Vector3 &element : target) { + element = expected.xform(element + translation_vector); + } + Vector weight = { 1.0, 1.0, 1.0 }; // Equal weights + bool translate = true; + + Quaternion result = qcp.weighted_superpose(moved, target, weight, translate); + CHECK(abs(result.x - expected.x) < epsilon); + CHECK(abs(result.y - expected.y) < epsilon); + CHECK(abs(result.z - expected.z) < epsilon); + CHECK(abs(result.w - expected.w) < epsilon); + + // Check if translation occurred + CHECK(translate); + Vector3 translation_result = expected.xform_inv(qcp.get_translation()); + CHECK(abs(translation_result.x - translation_vector.x) < epsilon); + CHECK(abs(translation_result.y - translation_vector.y) < epsilon); + CHECK(abs(translation_result.z - translation_vector.z) < epsilon); +} + +TEST_CASE("[Modules][QCP] Weighted Translation Shortest Path") { + double epsilon = CMP_EPSILON; + QCP qcp(epsilon); + + Quaternion expected = Quaternion(1, 2, 3, 4).normalized(); + PackedVector3Array moved = { Vector3(4, 5, 6), Vector3(7, 8, 9), Vector3(1, 2, 3) }; + PackedVector3Array target = moved; + Vector3 translation_vector = Vector3(1, 2, 3); + for (Vector3 &element : target) { + element = expected.xform(element + translation_vector); + } + Vector weight = { 1.0, 1.0, 1.0 }; // Equal weights + bool translate = true; + + Quaternion result = qcp.weighted_superpose(moved, target, weight, translate); + CHECK(abs(result.x - expected.x) > epsilon); + CHECK(abs(result.y - expected.y) > epsilon); + CHECK(abs(result.z - expected.z) > epsilon); + CHECK(abs(result.w - expected.w) > epsilon); + + // Check if translation occurred + CHECK(translate); + Vector3 translation_result = expected.xform_inv(qcp.get_translation()); + CHECK(abs(translation_result.x - translation_vector.x) > epsilon); + CHECK(abs(translation_result.y - translation_vector.y) > epsilon); + CHECK(abs(translation_result.z - translation_vector.z) > epsilon); +} +} // namespace TestQCP + +#endif // TEST_QCP_H